#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/wdt.h> // WatchDog          
#include <stdint.h>
#include <string.h>

#include "Ethernet/socket.h"
#include "Ethernet/wizchip_conf.h"

#include "Internet/MQTT/mqtt_interface.h"
#include "Internet/MQTT/MQTTClient.h"

#include "avrIOhelper/io-helper.h"
#include "kraftsensor.h"
#include "millis.h"
#include "uart.h"
#include "spi.h"
#include "mqtt.h"
#include "modbus.h"
#include "modbus-master.h"

#include "util/delay.h"

#define PLC_MQTT_ENABLED 1

Client mqtt_client;

//***********Prologue for fast WDT disable & and save reason of reset/power-up: BEGIN
uint8_t mcucsr_mirror __attribute__ ((section (".noinit")));

// This is for fast WDT disable & and save reason of reset/power-up
void get_mcusr(void) \
         __attribute__((naked)) \
         __attribute__((section(".init3")));
void get_mcusr(void)
{
    mcucsr_mirror = MCUSR;
    MCUSR = 0;
    wdt_disable();
}
//***********Prologue for fast WDT disable & and save reason of reset/power-up: END


//FUNC headers
static void avr_init(void);
void timer0_init(void);
void print_network_information(void);

volatile uint16_t ADC_reading = 0;

void IO_LIBRARY_Init(void) {
    uint8_t bufSize[] = {2, 2, 2, 2, 2, 2, 2, 2};

    reg_wizchip_cs_cbfunc(spi_select, spi_deselect);
    reg_wizchip_spi_cbfunc(spi_read, spi_write);
    //reg_wizchip_spiburst_cbfunc(spi_rb_burst, spi_wb_burst);

    wizchip_init(bufSize, bufSize);
    wizchip_setnetinfo(&netInfo);
    //wizchip_setinterruptmask(IK_SOCK_0);
}

void do_notaus(){
    if(read_Input(IN_NOTAUS_ANLAGE, LEVEL) || read_Input(IN_NOTAUS_SCHRANK, LEVEL) || read_Input(IN_NOTAUS_ANLAGE_R, LEVEL)){
        /* at least one pressed */
        //notaus_state = POWER_OFF;

        ioHelperSetBit(outStates, LED_AMPEL_ROT,  1);
        ioHelperSetBit(outStates, LED_AMPEL_GELB,  0);
        ioHelperSetBit(outStates, LED_AMPEL_GRUEN,  0);

        set_Output(LED_GRN_NOTAUS_SCHRANK,   OFF);
        set_Output(LED_GRN_NOTAUS_ANLAGE,    OFF);
        set_Output(LED_GRN_NOTAUS_ANLAGE_R,   OFF);

        if(read_Input(IN_NOTAUS_ANLAGE, LEVEL)){
            set_Output(LED_ROT_NOTAUS_ANLAGE, BLINK);
            set_Output(LED_ROT_NOTAUS_SCHRANK,   ON);
            set_Output(LED_ROT_NOTAUS_ANLAGE_R,   ON);
        }

        if(read_Input(IN_NOTAUS_SCHRANK, LEVEL)){
            set_Output(LED_ROT_NOTAUS_ANLAGE,    ON);
            set_Output(LED_ROT_NOTAUS_SCHRANK, BLINK);
            set_Output(LED_ROT_NOTAUS_ANLAGE_R,   ON);
        }

        if(read_Input(IN_NOTAUS_ANLAGE_R, LEVEL)){
            set_Output(LED_ROT_NOTAUS_ANLAGE,    ON);
            set_Output(LED_ROT_NOTAUS_SCHRANK,   ON);
            set_Output(LED_ROT_NOTAUS_ANLAGE_R, BLINK);
        }

    }
    else if(!read_Input(IN_ANLAGE_EIN_INV, LEVEL)){
        /* nothing pressed, but power not on */
        //notaus_state = ARMED;

        ioHelperSetBit(outStates, LED_AMPEL_ROT,  0);
        ioHelperSetBit(outStates, LED_AMPEL_GELB,  1);
        ioHelperSetBit(outStates, LED_AMPEL_GRUEN,  0);

        set_Output(LED_ROT_NOTAUS_ANLAGE,  ON);
        set_Output(LED_ROT_NOTAUS_SCHRANK, ON);
        set_Output(LED_ROT_NOTAUS_ANLAGE_R, ON);

        set_Output(LED_GRN_NOTAUS_SCHRANK, ON);
        set_Output(LED_GRN_NOTAUS_ANLAGE,  ON);
        set_Output(LED_GRN_NOTAUS_ANLAGE_R, ON);
    }
    else{
        /* powered on */
        //notaus_state = POWER_ON;

        ioHelperSetBit(outStates, LED_AMPEL_ROT,  0);
        ioHelperSetBit(outStates, LED_AMPEL_GELB,  0);
        ioHelperSetBit(outStates, LED_AMPEL_GRUEN,  1);

        set_Output(LED_GRN_NOTAUS_SCHRANK,   ON);
        set_Output(LED_GRN_NOTAUS_ANLAGE,    ON);
        set_Output(LED_GRN_NOTAUS_ANLAGE_R,   ON);

        set_Output(LED_ROT_NOTAUS_ANLAGE,    OFF);
        set_Output(LED_ROT_NOTAUS_SCHRANK,   OFF);
        set_Output(LED_ROT_NOTAUS_ANLAGE_R,   OFF);
    }

}

void send_values(void){
    char msg[10];

    sprintf(msg, "%d", ADC_reading);
    mqtt_pub(&mqtt_client, "/Filamentanlage/01_Extruder/state/temp", msg, strlen(msg));

}

void modbus_set_Output(uint8_t slave_adr, uint8_t nr, uint8_t state) {
  switch (state) {
    case BLINK:
        //ioHelperSetBit(outStatesBlinking,  nr,  ON);
        writeCoil(slave_adr,100+nr,1);
        wait_write(10);
        break;
    //case TOGGLE:
    //  ioHelperSetBit(outStatesBlinking,  nr,  OFF);
    //  if (ioHelperReadBit(outStates, nr)) {
    //    ioHelperSetBit(outStates,  nr,  OFF);
    //  } else {
    //    ioHelperSetBit(outStates,  nr,  ON);
    //  }
    //  break;
    case ON:
      //ioHelperSetBit(outStates,  nr,  ON);
      //ioHelperSetBit(outStatesBlinking,  nr,  OFF);
        writeCoil(slave_adr,nr,1);
        wait_write(10);
      break;
    case OFF:
        writeCoil(slave_adr,nr,0);
        wait_write(10);
      //ioHelperSetBit(outStates,  nr,  OFF);
      //ioHelperSetBit(outStatesBlinking,  nr,  OFF);
      break;
  }
}

uint8_t modbus_read_Input(uint8_t nr, uint8_t type) {
  uint8_t state = 0;
  switch (type) {
    case LEVEL:
      state = ioHelperReadBit(inStates, nr);
      break;
    case EDGE:
      state = ioHelperReadBit(inStatesBothEdges, nr);
      ioHelperSetBit(inStatesBothEdges,  nr,  0);
      break;
    case RISING:
      state = ioHelperReadBit(inStatesRisingEdge, nr);
      ioHelperSetBit(inStatesRisingEdge,  nr,  0);
      break;
    case FALLING:
      state = ioHelperReadBit(inStatesFallingEdge, nr);
      ioHelperSetBit(inStatesFallingEdge,  nr,  0);
      break;
  }

  //reset edges after being read once
  // if (state) {
  //   if (type == EDGE)
  //     ioHelperSetBit(inStatesBothEdges,  nr,  OFF);
  //   if (type == RISING)
  //     ioHelperSetBit(inStatesRisingEdge,  nr,  OFF);
  //   if (type == FALLING)
  //     ioHelperSetBit(inStatesFallingEdge,  nr,  OFF);
  // }

  return state;
}



int main(){
    // INIT MCU
    avr_init();
    spi_init(); //SPI Master, MODE0, 4Mhz(DIV4), CS_PB.3=HIGH - suitable for WIZNET 5x00(1/2/5)
    //spi_speed_tst(); / Here on SPI pins: MOSI 400Khz freq out, on SCLK 3.2MhzOUT (Witk SPI CLK 4Mhz)

    ioHelperInitBuffer();
    ioHelperIoConf();

    //Wizchip WIZ5500 Ethernet initialize
    IO_LIBRARY_Init(); //After that ping must working
    print_network_information();

#if PLC_MQTT_ENABLED
    //****************MQTT client initialize
    //Find MQTT broker and connect with it
    uint8_t mqtt_buf[100];
    int32_t mqtt_rc = 0;
    Network mqtt_network;
    mqtt_network.my_socket = SOCK_MQTT;

    printf(">>Trying connect to MQTT broker: %d.%d.%d.%d ..\r\n", MQTT_targetIP[0], MQTT_targetIP[1], MQTT_targetIP[2], MQTT_targetIP[3]);
    NewNetwork(&mqtt_network);
    ConnectNetwork(&mqtt_network, MQTT_targetIP, 1883);
    MQTTClient(&mqtt_client, &mqtt_network, 1000, mqtt_buf, 100, mqtt_readBuffer, MQTT_BUFFER_SIZE);

    //Connection to MQTT broker
    MQTTPacket_connectData data = MQTTPacket_connectData_initializer;
    data.willFlag = 0;
    data.MQTTVersion = 4;//3;
    data.clientID.cstring = (char*)"controllino_wasserbecken";
    data.username.cstring = (char*)"Wasserbecken";
    data.password.cstring = (char*)"\0";
    data.keepAliveInterval = 10;
    data.cleansession = 1;
    mqtt_rc = MQTTConnect(&mqtt_client, &data);
    if (mqtt_rc == SUCCESSS)
    {
        printf("++MQTT Connected SUCCESS: %ld\r\n", mqtt_rc);
    }
    else
    {
        printf("--MQTT Connected ERROR: %ld\r\n", mqtt_rc);
        while(1); //Reboot the board
    }

    // Subscribe to all topics
    char SubString[] = "/Filamentanlage/01_Extruder/set/#";
    //char SubString[] = "/Filamentanlage/03_Wasserbecken/#";
    mqtt_rc = MQTTSubscribe(&mqtt_client, SubString, QOS0, messageArrived);
    printf("Subscribed (%s) %ld\r\n", SubString, mqtt_rc);
#endif


    ioHelperSetBit(outStatesBlinking,  LED_PLC_OK,  1);
    ioHelperSetBit(outStates,  BitPG5,  1);

    uint32_t timer_blink_outs = millis();
    uint32_t timer_send_uptime = millis();
    uint32_t timer_send_info = millis();

    //printf("anlage: %x\n\r", read_Input(IN_ANLAGE_EIN_INV, LEVEL));

    uint16_t tgl = 0;
    while(1)
    {
        wdt_reset(); // WDT reset at least every sec

        ioHelperReadPins();
        ioHelperDebounce();
        ioHelperEdgeDetector();

        // Toggle all outs which are set to blinking
        if(millis() - timer_blink_outs > 500){
            outStates[0] ^= outStatesBlinking[0];
            outStates[1] ^= outStatesBlinking[1];
            outStates[2] ^= outStatesBlinking[2];
            outStates[3] ^= outStatesBlinking[3];
            timer_blink_outs = millis();

            do_kraftsensor();

            char msg[64];
            sprintf(msg, "%d", kraftsensor_value);
            mqtt_pub(&mqtt_client, "/Filamentanlage/01_Extruder/kraft", msg, strlen(msg));

            //tgl ^= 0x01;
            //modbus_set_Output(4, 0, OFF);
            //modbus_set_Output(4, 1, ON);
            //modbus_set_Output(4, 2, BLINK);
            //modbus_set_Output(4, 3, tgl);

            //readInputReg(4, 0, 1);
            //uint16_t inp;
            //wait_receive(1, &inp, 100);
            //printf("read inputs: 0x%X\n", inp);
        }

#if PLC_MQTT_ENABLED
        // send misc info
        if(millis() - timer_send_uptime > 5000){
            timer_send_uptime += 5000;
            char msg[64];
            sprintf(msg, "%ld", millis()/1000);
            mqtt_pub(&mqtt_client, "/Filamentanlage/01_Extruder/uptime", msg, strlen(msg));
        }
#endif

#if PLC_MQTT_ENABLED
        // send misc info
        if(millis() - timer_send_info > 200){
            timer_send_info += 200;
            send_values(); // 10ms
            //send_info(); // 27ms every 200ms
        }
#endif

        //if(read_Input(IN_ANLAGE_EIN_INV, FALLING)){
        //    printf("anlage ein\n\r");
        //}
        //    ioHelperSetBit(outStates, LED_GRN_NOTAUS_SCHRANK, 1);
        //}
        //else{
        //    ioHelperSetBit(outStates, LED_GRN_NOTAUS_SCHRANK, 0);
        //}
        do_notaus();

#if PLC_MQTT_ENABLED
        ioHelperSetBit(outStates, LED_BUS_OK, 1);
        ioHelperSetOuts();
        MQTTYield(&mqtt_client, 10); //blocking call
        ioHelperSetBit(outStates, LED_BUS_OK, 0);
#endif
        ioHelperSetOuts();
}
return 0;
}

// Timer0
// 1ms IRQ
// Used for millis() timing
void timer0_init(void)
{
    TCCR0A = (1<<WGM01); //TIMER0 SET-UP: CTC MODE
    TCCR0B = (1<<CS01)|(1<<CS00); // PS 1:64
    OCR0A = 249; // 1ms reach for clear (16mz:64=>250kHz:250-=>1kHz)
    TIMSK0 |= 1<<OCIE0A;	 //IRQ on TIMER0 output compareA
}

void timer1_init()
{
    TCCR3A |= (1<<COM3B1);
    TCCR3B |= _BV(WGM33);

    ICR3 = 100;
    OCR3A = 50;

    DDRE |= 1 << 4;
    TIMSK3 |= 1<<TOIE3;

    TCCR3B |= _BV(CS31);
}

void timer3_init()
{
    TCCR3A |= (1<<COM3B1);
    TCCR3B |= _BV(WGM33);

    ICR3 = 100;
    OCR3B = 50;

    DDRE |= 1 << 4;
    TCCR3B |= _BV(CS31);
}

void initADC(void)
{
    ADMUX = 1 << REFS0 | 0 << REFS1; //Select external Vref
    ADCSRA = _BV(ADEN) | _BV(ADIE); // enable adc, enable interrupt
    ADCSRA |= 1 << ADPS2 | 1 << ADPS1 | 1 << ADPS0; // set clock-prescaler to 128
    ADCSRA |= 1 << ADSC; // start conversion
}

static void avr_init(void)
{
    // Initialize device here.
    // WatchDog INIT
    wdt_enable(WDTO_8S);  // set up wdt reset interval 2 second
    wdt_reset(); // wdt reset ~ every <2000ms

    timer0_init();// Timer0 millis engine init
    //timer3_init();
    DDRL |= 1<<6;
    uart_init();

    initADC();

    modbus_master_init();

    sei(); //re-enable global interrupts

    return;
}



void print_network_information(void)
{

    uint8_t tmpstr[6] = {0,};
    ctlwizchip(CW_GET_ID,(void*)tmpstr); // Get WIZCHIP name
    printf("\r\n=======================================\r\n");
    printf(" WIZnet chip:  %s \r\n", tmpstr);
    printf("=======================================\r\n");

    wiz_NetInfo gWIZNETINFO;
    wizchip_getnetinfo(&gWIZNETINFO);
    if (gWIZNETINFO.dhcp == NETINFO_STATIC)
        printf("STATIC IP\r\n");
    else
        printf("DHCP IP\r\n");
    printf("Mac address: %02x:%02x:%02x:%02x:%02x:%02x\n\r",gWIZNETINFO.mac[0],gWIZNETINFO.mac[1],gWIZNETINFO.mac[2],gWIZNETINFO.mac[3],gWIZNETINFO.mac[4],gWIZNETINFO.mac[5]);
    printf("IP address : %d.%d.%d.%d\n\r",gWIZNETINFO.ip[0],gWIZNETINFO.ip[1],gWIZNETINFO.ip[2],gWIZNETINFO.ip[3]);
    printf("SM Mask	   : %d.%d.%d.%d\n\r",gWIZNETINFO.sn[0],gWIZNETINFO.sn[1],gWIZNETINFO.sn[2],gWIZNETINFO.sn[3]);
    printf("Gate way   : %d.%d.%d.%d\n\r",gWIZNETINFO.gw[0],gWIZNETINFO.gw[1],gWIZNETINFO.gw[2],gWIZNETINFO.gw[3]);
    printf("DNS Server : %d.%d.%d.%d\n\r",gWIZNETINFO.dns[0],gWIZNETINFO.dns[1],gWIZNETINFO.dns[2],gWIZNETINFO.dns[3]);
}

ISR(ADC_vect)
{
    //Reading 10bit conversion result
    ADC_reading = ADCL;       //copy the first LSB bits
    ADC_reading |= ADCH << 8; //copy remaing byte
    ADCSRA |= (1 << ADSC); //Start next conversion
}

