#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "slaveconfig.h"
#include "ethercat.h"

#define EC_TIMEOUTMON 100

char IOmap[4096];
volatile int wkc;
OSAL_THREAD_HANDLE thread1;
int expectedWKC;
boolean needlf;
volatile int wkc;
uint8 currentgroup = 0;


/*!
 * @brief Set the given Slave to given State 
 * @return 1 if No Error | 0 if Error
 * */
uint8_t setSlaveState(uint8_t slave_index, uint16_t ethercat_state )
{
	//printf("SET STATE: %s - Slave[%d] \n",ec_slave[slave_index].name, slave_index);
	ec_slave[slave_index].state = ethercat_state;// ec_slave[0] refers to a special virtual slave that represents all slaves 
	ec_writestate(slave_index); // Same as above
	ec_statecheck(slave_index, ethercat_state,  EC_TIMEOUTSTATE);

	if (ec_slave[slave_index].state == ethercat_state) {
		{
	//		printf("  |- ");
	//		printState(ec_slave[slave_index].state);
		}
		return 1;
	} else {
	//	printf("  |-> Failed\n");
		return 0; 
	}
	return 0; 
}

/*
void searchForSlave()
{
	for (int i = 1; i <= ec_slavecount; i++) {
	    if (strcmp(ec_slave[i].name, "EL2008") == 0) {
		el2008_slave = i;
		printf("Found EL2008 at slave %d.\n", i);
	    }
	}

	if (el2008_slave == -1) {
	    printf("EL2008 not found on the network.\n");
	}
}
*/


void print_device_info() {
	printf("--------------------------------------------------\n");
    printf("EtherCAT Device Info Panel\n");
    printf("--------------------------------------------------\n");

    for (int i = 1; i <= ec_slavecount; i++) {
        printf("Slave %d: %s\n", i, ec_slave[i].name);
        printf("  Vendor ID: 0x%08X\n", ec_slave[i].eep_man);
        printf("  Product Code: 0x%08X\n", ec_slave[i].eep_id);
        printf("  Revision: 0x%08X\n", ec_slave[i].eep_rev);
		printf("  ");
        printState(ec_slave[i].state);
        printf("  AL Status Code: 0x%04X\n", ec_slave[i].ALstatuscode);

        // Print Process Data (PDOs)
        printf("  Process Data:\n");
        printf("    Inputs (%d bytes): ", ec_slave[i].Ibytes);
        for (uint32_t j = 0; j < ec_slave[i].Ibytes; j++) {
            printf("%02X ", ec_slave[i].inputs[j]);
        }
        printf("\n");

        printf("    Outputs (%d bytes): ", ec_slave[i].Obytes);
        for (uint32_t j = 0; j < ec_slave[i].Obytes; j++) {
            printf("%02X ", ec_slave[i].outputs[j]);
        }
        printf("\n");

        // Read available SDOs
        printf("  SDO List:\n");
        for (int sdo_index = 0x1000; sdo_index <= 0x1FFF; sdo_index++) {
            uint32_t sdo_value;
            int size = sizeof(sdo_value);
            if (ec_SDOread(i, sdo_index, 0x00, FALSE, &size, &sdo_value, EC_TIMEOUTSAFE) > 0) {
                printf("    Index 0x%04X: Value 0x%08X\n", sdo_index, sdo_value);
            }
        }
        printf("--------------------------------------------------\n");
    }
}


void setPin1Out()
	{
    uint8_t *outputs = (uint8_t *)ec_slave[3].outputs;

    // Print current output state before modification
    printf("Current output state: 0x%02X\n", outputs[0]);

    // Set bit 0 (Channel 1 ON)
    outputs[0] |= 1;

    // Send process data to the slave
    ec_send_processdata();

    wkc = ec_receive_processdata(EC_TIMEOUTRET);

    if (wkc >= expectedWKC) {
        printf("Process data exchanged successfully.\n");
    } else {
        printf("Process data exchange failed. WKC: %d, expectedWKC: %d\n", wkc, expectedWKC);
    }

    // Print final output state
    printf("Final output state: 0x%02X\n", outputs[0]);

    // Print slave state and AL status code
	printState(ec_slave[3].state);
}


/*!
 * @brief Initialising Distributed Clocks. you should initialize Distributed Clocks (DC) 
 * after all slaves are configured and mapped, 
 * but before transitioning them to OPERATIONAL (EC_STATE_OPERATIONAL).
 * @return 1 if No Error | 0 if Error
 * */
uint8_t initDisributedClocks()
{
	printf("Init Distributed Clocks\n");
	if (ec_configdc()) {
		printf("  |-> Succeeded \n");
		return 1;
	}else 
	{
		printf("  |-> Failed \n");
		return 0;
	}
}


/*!
 * @brief map process data objects (PDOs) of all detected EtherCAT slaves into a single memory buffer (IOmap). 
 * and groupe them into group[0] This will be important for the Slaves state intitalisation phase.  
 * @return 1 if No Error | 0 if Error
 * */
uint8_t mapSlavesGrouped()
{
	printf("Mapping Slaves\n");
	if(ec_config_map(&IOmap)){
		printf("  |-> Succeeded \n");
		for (uint8_t i = 1; i <= ec_slavecount; i++) {
			printf("   |-> (%s) - ",ec_slave[i].name);
			printState(ec_slave[i].state);
		}
		return 1;
	}else 
	{
		printf("  |-> Failed \n");
		return 0;
	}
}

/*!
 * @brief Enumerate and init all slaves.> 
 * @return 1 if No Error | 0 if Error
 * */
uint8_t enumerateSlaves()
{
	printf("Detection and enumeration \n");
	if (ec_config_init(FALSE) > 0) {
		printf("  |-> Succeeded \n");
		printf("   |-> Slave Count: %d \n",ec_slavecount);
		return 1;
	}else 
	{
		printf("  |-> Failed \n");
		return 0;
	}
}	

/*!
 * @brief Initialising network atapter for communication with slaves
 * @param char *Ifname is the interface name
 * @return 1 if No Error | 0 if Error
 * */
uint8_t initNetworkAdapterSingle(char *ifname)
{
	printf("Init Network Interface < %s >\n", ifname);
	if (ec_init(ifname)) {
		printf("  |-> Succeeded \n");
		return 1;
	}else 
	{
		printf("  |-> Failed \n");
		return 0;
	}
}

void dataLoop()
{
	ec_send_processdata(); 
	wkc = ec_receive_processdata(EC_TIMEOUTRET); 
	if(wkc >= expectedWKC)
	{
		
	}
	osal_usleep(5000);
}

int main(int argc, char *argv[]) {
	if (argc > 1) {
		// Init Network adapter
		if(!initNetworkAdapterSingle(argv[1])){
			printf("Unable to Initiate Network Adapter -> Quitting");
			ec_close();
			return 0;
		}
		
		if(!enumerateSlaves()){
			printf("Unable to Enumerate Slaves -> Quitting");
			ec_close();
			return 0;
		}

		if(!mapSlavesGrouped()){
			printf("Unable to Map Slaves -> Quitting");
			ec_close();
			return 0;
		}
		
		expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
		printf("Calculated workcounter %d\n", expectedWKC);
		
		if(!initDisributedClocks()){
			printf("Unable to Initialise Distributed Clocks -> Quitting");
			ec_close();
			return 0;
		}

		for (uint8_t i = 1; i <= ec_slavecount; i++) {
			printf("-----------------------------------------------------------------------\n"); 
			print_device_info();
			printf("-----------------------------------------------------------------------\n"); 
			if(!setSlaveState(1,EC_STATE_INIT)){
				printf("Unable chnage Slave mode -> Quitting");
				ec_close();
				return 0;
			}
			if(!setSlaveState(i,EC_STATE_PRE_OP)){
				printf("Unable chnage Slave mode -> Quitting");
				ec_close();
				return 0;
			}
			if(!setSlaveState(i,EC_STATE_SAFE_OP)){
				printf("Unable chnage Slave mode -> Quitting");
				ec_close();
				return 0;
			}
			if(!setSlaveState(i,EC_STATE_OPERATIONAL)){
				printf("Unable chnage Slave mode -> Quitting");
				ec_close();
			}
			printf("-----------------------------------------------------------------------\n"); 
		}

		 setPin1Out();

		 for(uint16_t j = 0; j < 10000; j++) 
		 {
			ec_send_processdata();
			wkc = ec_receive_processdata(EC_TIMEOUTRET);

			if (wkc >= expectedWKC) {
			} else {
			printf("Process data exchange failed. WKC: %d, expectedWKC: %d\n", wkc, expectedWKC);
			}
			 usleep(100);
		 }

		ec_close();
	} else {
		printf("Usage   : sudo %s <network interface name>  need to be passed\n", argv[0]);
		printf("Example : sudo %s eth0 \n", argv[0]);
	}
	return 0;
}