#include <rtai_mbx.h>
#include <rtai_sched.h>
#include <rtai_sem.h>

#include <uint128.h>
#include <sys/rtai_modbus.h>

#define SENSOR_PART_TURNTABLE 1<<0
#define SENSOR_PART_DRILL 1<<1
#define SENSOR_PART_TESTER 1<<2
#define SENSOR_DRILL_UP 1<<3
#define SENSOR_DRILL_DOWN 1<<4
#define SENSOR_TURNTABLE_POS 1<<5
#define SENSOR_PART_TEST 1<<6

#define ACTOR_DRILL 1<<0
#define ACTOR_TURNTABLE 1<<1
#define ACTOR_DRILL_DOWN 1<<2
#define ACTOR_DRILL_UP 1<<3
#define ACTOR_PART_HOLD 1<<4
#define ACTOR_TESTER 1<<5
#define ACTOR_EXIT 1<<6
#define ACTOR_ENTRANCE 1<<7

MODULE_LICENSE("GPL");

static RT_TASK turntable_task, drill_task, tester_task, output_task;
static MBX tester_mbx, output_mbx, drill_mbx, turntable_mbx;
static SEM semaphore;

int connection;

char node[] = "modbus-node";

void end(bool fail) {
    rt_modbus_disconnect(connection);
    rt_task_delete(&turntable_task);
    rt_task_delete(&tester_task);
    rt_task_delete(&drill_task);
    rt_task_delete(&output_task);

    rt_mbx_delete(&tester_mbx);
    rt_mbx_delete(&output_mbx);
    rt_sem_delete(&semaphore);

    stop_rt_timer();

    if(fail) {
        rt_printk("failed to read/send Modbus message.\n");
        rt_printk("module needs to be restarted.\n");
    }
}

/**
 * readData all bits of the specified type
 * this function is *not* thread safe.
 * @param type (DIGITAL_IN, DIGITAL_OUT, ANALOG_IN, ANALOG_OUT)
 * @param connection modbus connection id
 * @param result value to write the result to
 * @return status code(0: success, 1: failure)
 */
int readAll(int type, int *result) {
    int res = rt_modbus_get(connection, type, 0, (unsigned short *) result);
    //rt_printk("%d\n", res);
    //rt_printk("%d", &result);
    return res;
}

/**
 *
 * this function is thread safe.
 * @param part
 * @return
 */
int readData(int part) {
    rt_sem_wait(&semaphore);
    int value = 0;
    int code = readAll(DIGITAL_IN, &value);
    int result = (part & value);
    rt_sem_signal(&semaphore);
    if(code) end(true);
    return result;
}

/**
 * disable a specified actor of the system.
 * this function is thread safe.
 * @param connection modbus connection id
 * @param actor bitmask of the actor to disable
 */
void disable(int actor) {
    rt_sem_wait(&semaphore);
    int value = 0;
    int code = readAll(DIGITAL_OUT, &value);
    if(code) end(true);
    int result = rt_modbus_set(connection, DIGITAL_OUT, 0, value &= ~actor);
    rt_sem_signal(&semaphore);
    if (result) end(true);
}

/**
 * enable one specified actor of the system.
 * this function is thread safe.
 * @param connection modbus connection id
 * @param actor bitmask of the actor to enable
 */
void enable(int actor) {
    rt_sem_wait(&semaphore);
    int output = 0;
    int code = readAll(DIGITAL_OUT, &output);
    if (code) end(true);
    int result;
    result = rt_modbus_set(connection, DIGITAL_OUT, 0, output |= actor);
    rt_sem_signal(&semaphore);
    if(result) end(true);
}

void sleepStuff(void) {
    rt_sleep(1000 * nano2count(1000000));
}

static void turntable(long data) {
//	rt_printk("started turntable task");
//    disable(ACTOR_TURNTABLE);
//
//    while (1) {
//        int partOnTable = readData(SENSOR_PART_TURNTABLE);
//        rt_printk("%d", partOnTable);
//        if (partOnTable) {
//            enable(ACTOR_TURNTABLE);
//        } else {
//            disable(ACTOR_TURNTABLE);
//        }
//
//        sleepStuff();
//    }
	while (1) {

	        sleepStuff();
	    }

}

static void tester(long data) {

	disable(ACTOR_TESTER);
    while (1) {

    	int partOnTable = readData(SENSOR_PART_TESTER);
    	rt_printk("part: %d\n", partOnTable);
    	if(partOnTable) {
    		enable(ACTOR_TESTER);
    		sleepStuff();
    		int partNormal = readData(SENSOR_PART_TEST);
    		if(partNormal) {
    			//nachricht bohrer(ON)
    			int msg = 1;
    			int mbxStatus = rt_mbx_send(&drill_mbx, &msg, sizeof(int));
    			if(mbxStatus) end(true);
    		} else {
    			//nachricht bohrer(OFF)
    			int msg = 0;
    			int mbxStatus = rt_mbx_send(&drill_mbx, &msg, sizeof(int));
    			if(mbxStatus) end(true);
    		}
    		rt_printk("normal %d\n", partNormal);
    		disable(ACTOR_TESTER);
    		//Nachricht drehteller
    	}

        sleepStuff();
    }
}

static void drill(long data) {

    disable(ACTOR_DRILL);
    disable(ACTOR_PART_HOLD);
    disable(ACTOR_DRILL_DOWN);
    rt_printk("hello");

    enable(ACTOR_DRILL_UP);
    if (!readData(SENSOR_DRILL_UP)) {
        do {
        } while (readData(SENSOR_DRILL_UP) == 0);
    }
    disable(ACTOR_DRILL_UP);

    while (1) {
    	int part = readData(SENSOR_PART_DRILL);
    	if(part) {
    		int msg = 0;
    		int mbxStatus = rt_mbx_receive(&drill_mbx, &msg, sizeof(int));
    		if(mbxStatus) end(true);
    		if(msg) {
    			enable(ACTOR_PART_HOLD);
    			enable(ACTOR_DRILL);
    			enable(ACTOR_DRILL_DOWN);
    			if (!readData(SENSOR_DRILL_DOWN)) {
    				do {
    			    } while (readData(SENSOR_DRILL_DOWN) == 0);
    			}
    			sleepStuff();
				disable(ACTOR_PART_HOLD);
				disable(ACTOR_DRILL);
				disable(ACTOR_DRILL_DOWN);
				enable(ACTOR_DRILL_UP);
				if (!readData(SENSOR_DRILL_UP)) {
					do {
					} while (readData(SENSOR_DRILL_UP) == 0);
				}
				disable(ACTOR_DRILL_UP);
    		}
    	}
    	int msg = part;
    	int mbxStatus = rt_mbx_send(&output_mbx, &msg, sizeof(int));
    	if(mbxStatus) end(true);

        sleepStuff();
    }
}

static void output(long data) {

    while (1) {
    	//turntable msg

    	int msg = 0;
    	int mbxStatus = rt_mbx_receive(&output_mbx, &msg, sizeof(int));
    	if(mbxStatus) end(true);

    	if(msg) {
    		enable(ACTOR_EXIT);
    		sleepStuff();
    		disable(ACTOR_EXIT);
    	}
    	//nachricht an turntable

        sleepStuff();
    }
}

static int __init
example_init(void) {
    rt_set_oneshot_mode();
    start_rt_timer(0);
    modbus_init();

    rt_printk("init: task started\n");
    if ((connection = rt_modbus_connect(node)) == -1) {
        rt_printk("init: could not connect to %s\n", node);
        return 1;
    }

    rt_sem_init(&semaphore, 1);

    if (rt_task_init(&turntable_task, turntable, 0, 1024, 0, 0, NULL)) {
        printk("turntable: cannot initialize task\n");
        goto fail;
    }
    if (rt_task_init(&drill_task, drill, 0, 1024, 0, 0, NULL)) {
        printk("drill: cannot initialize task\n");
        goto fail;
    }
    if (rt_task_init(&output_task, output, 0, 1024, 0, 0, NULL)) {
            printk("output: cannot initialize task\n");
            goto fail;
    }
    if (rt_task_init(&tester_task, tester, 0, 1024, 0, 0, NULL)) {
        printk("tester: cannot initialize task\n");
        goto fail;
    }

    if (rt_mbx_init(&tester_mbx, sizeof(int))) {
    	end(true);
    }
    if (rt_mbx_init(&output_mbx, sizeof(int))) {
        end(true);
    }
    if (rt_mbx_init(&drill_mbx, sizeof(int))) {
        end(true);
    }
    if (rt_mbx_init(&output_mbx, sizeof(int))) {
        end(true);
    }
    if (rt_mbx_init(&turntable_mbx, sizeof(int))) {
        end(true);
    }

    rt_task_resume(&turntable_task);
    rt_task_resume(&drill_task);
    rt_task_resume(&output_task);
    rt_task_resume(&tester_task);
    printk("loaded module Bearbeiten2\n");
    return (0);

    fail:
    end(true);
    return (1);
}

static void __exit
example_exit(void) {
    end(false);
    printk("module Bearbeiten2 unloaded\n");
}

module_exit(example_exit)
module_init(example_init)