/*    This is a hardware step generator with dual core ESP32 and W5500 module
 *    for LinuxCNC
 *
 *    Copyright 2021 Juhász Zoltán <juhasz.zoltan at freemail dot hu>
 *
 *    This program is free software; you can redistribute it and/or modify
 *    it under the terms of the GNU General Public License as published by
 *    the Free Software Foundation; either version 2 of the License, or
 *    (at your option) any later version.
 *
 *    This program is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *    GNU General Public License for more details.
 *
 *    You should have received a copy of the GNU General Public License
 *    along with this program; if not, write to the Free Software
 *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

/*    Hardware:
 *
 *           3v3 -- 3.3v W5500
 *           GND -- GND  W5500
 *           GND -- (DB15- 7)
 *           GND -- (DB15- 8)
 *
 *       GPIO  2 -> -[100R]-(DB15 - 9) OUT-00 / onboard blue LED
 *       GPIO  4 -> -[100R]-(DB15 -10) OUT-01
 *       GPIO  5 -> W5500 SCS
 *       GPIO 12 -> -[100R]-(DB15 - 1) step-0
 *       GPIO 13 -> -[100R]-(DB15 - 2) dir-0 
 * (RX2) GPIO 16 -> -[100R]-(DB15 - 3) step-1
 * (TX2) GPIO 17 -> -[100R]-(DB15 - 4) dir-1
 *       GPIO 18 -> W5500 SCLK
 *       GPIO 19 <- W5500 MISO
 *       GPIO 21 -> -[100R]-(DB15 - 5) step-2
 *       GPIO 22 -> -[100R]-(DB15 - 6) dir-2
 *       GPIO 23 -> W5500 MOSI
 *       GPIO 25 -> -[100R]-(DB15 -11) OUT-02
 *       GPIO 26 <- -[100R]-(DB15 -12) IN-00
 *       GPIO 27 <- -[100R]-(DB15 -13) IN-01
 *       GPIO 32 <- -[100R]-(DB15 -14) IN-02
 *       GPIO 33 <- -[100R]-(DB15 -15) IN-03
 *       GPIO 34 <- NC {no pullup!}
 *       GPIO 35 <- NC {no pullup!}
 *  (VP) GPIO 36 <- NC {no pullup!}
 *  (VN) GPIO 39 <- NC {no pullup!}
 */

#include <arduino.h>
#include <Ethernet.h>
#include <EthernetUdp.h>

/*==================================================================*/

byte mac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
IPAddress ip(192, 168, 96, 54);
unsigned int port = 58427;

/*==================================================================*/

#define SPI_CS_PIN 5

/*==================================================================*/

#define STEP_0_PIN 12
#define STEP_0_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT12)
#define STEP_0_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT12)
#define DIR_0_PIN 13
#define DIR_0_PLUS REG_WRITE(GPIO_OUT_W1TS_REG, BIT13)
#define DIR_0_MINUS REG_WRITE(GPIO_OUT_W1TC_REG, BIT13)

#define STEP_1_PIN 16
#define STEP_1_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT16)
#define STEP_1_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT16)
#define DIR_1_PIN 17
#define DIR_1_PLUS REG_WRITE(GPIO_OUT_W1TS_REG, BIT17)
#define DIR_1_MINUS REG_WRITE(GPIO_OUT_W1TC_REG, BIT17)

#define STEP_2_PIN 21
#define STEP_2_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT21)
#define STEP_2_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT21)
#define DIR_2_PIN 22
#define DIR_2_PLUS REG_WRITE(GPIO_OUT_W1TS_REG, BIT22)
#define DIR_2_MINUS REG_WRITE(GPIO_OUT_W1TC_REG, BIT22)

/*==================================================================*/

#define OUT_00_PIN 2
#define OUT_00_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT2)
#define OUT_00_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT2)
#define OUT_01_PIN 4
#define OUT_01_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT4)
#define OUT_01_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT4)
#define OUT_02_PIN 25
#define OUT_02_H REG_WRITE(GPIO_OUT_W1TS_REG, BIT25)
#define OUT_02_L REG_WRITE(GPIO_OUT_W1TC_REG, BIT25)

/*==================================================================*/

#define IN_00_PIN 26
#define IN_00 REG_READ(GPIO_IN_REG) & BIT26 //26
#define IN_01_PIN 27
#define IN_01 REG_READ(GPIO_IN_REG) & BIT27 //27
#define IN_02_PIN 32
#define IN_02 REG_READ(GPIO_IN1_REG) & BIT0 //32 -32
#define IN_03_PIN 33
#define IN_03 REG_READ(GPIO_IN1_REG) & BIT1 //33 -32
#define IN_04_PIN 34
#define IN_04 REG_READ(GPIO_IN1_REG) & BIT2 //34 -32
#define IN_05_PIN 35
#define IN_05 REG_READ(GPIO_IN1_REG) & BIT3 //35 -32
#define IN_06_PIN 36
#define IN_06 REG_READ(GPIO_IN1_REG) & BIT4 //36 -32
#define IN_07_PIN 39
#define IN_07 REG_READ(GPIO_IN1_REG) & BIT7 //39 -32

/*==================================================================*/

#define CTRL_DIRSETUP 0b00000001
#define CTRL_MAXVEL   0b00000010
#define CTRL_MAXACC   0b00000100
#define CTRL_READY    0b01000000
#define CTRL_ENABLE   0b10000000

#define IO_00 0b00000001
#define IO_01 0b00000010
#define IO_02 0b00000100
#define IO_03 0b00001000
#define IO_04 0b00010000
#define IO_05 0b00100000
#define IO_06 0b01000000
#define IO_07 0b10000000

/*==================================================================*/

char packetBuffer[50]; // buffer for receiving and sending data

EthernetUDP Udp; // An EthernetUDP instance to let us send and receive packets over UDP

struct cmdPacket {
    uint8_t control;
    uint8_t io;
    int32_t pos[3];
    float vel[3];
} cmd = { 0, 0, 0, 0, 0, 0.0f, 0.0f, 0.0f };

struct fbPacket {
    uint8_t control;
    uint8_t io;
    int32_t pos[3];
    float vel[3];
} fb = { 0, 0, 0, 0, 0, 0.0f, 0.0f, 0.0f };

/*==================================================================*/

volatile unsigned long ul_dirSetup[3] = { 1000, 1000, 1000 }; // x 25 nanosec
volatile unsigned long ul_min_T[3] = { 10000, 10000, 10000 }; // x 25 nanosec
volatile unsigned long ul_maxAcc_x2[3] = { 1, 1, 1 }; // max acceleration step/sec2

/*==================================================================*/

volatile long l_predictive_pos[3];
volatile long l_cmd_pos_last[3];
volatile long l_cmd_pos[3] = { 0, 0, 0 };

volatile unsigned long ul_cmd_T[3];
volatile unsigned long ul_accelStep[3] = { 0, 0, 0 };
volatile unsigned long ul_T[3] = { 0, 0, 0 };
volatile unsigned long ul_TH[3] = { 0, 0, 0 };
volatile unsigned long ul_TL[3] = { 0, 0, 0 };
volatile bool b_math[3] = { false, false, false };
volatile bool b_dirSignal[3] = { LOW, LOW, LOW };
volatile bool b_dirChange[3] = { false, false, false };

unsigned long ul_watchdog;

hw_timer_t* stepGen_0 = NULL;
hw_timer_t* stepGen_1 = NULL;
hw_timer_t* stepGen_2 = NULL;

/*==================================================================*/

void IRAM_ATTR onTime_0()
{
    volatile static bool b_stepState = LOW;
    volatile static bool b_dirState = LOW;

    if (b_stepState == LOW) { // end of period
        if (ul_T[0]) { // van periódisidő tehát impulzust kell indítani
            if (!b_dirChange[0]) { //jó az irány, új impulzus indítása step L-H átmenet
                STEP_0_H;
                timerAlarmWrite(stepGen_0, ul_TH[0], true);
                b_stepState = HIGH;
                b_dirState ? fb.pos[0]++ : fb.pos[0]--;
                b_math[0] = true; // kiszámítjuk a következő értékeket
            } else { // irányt kell váltani
                if (b_dirSignal[0]) {
                    DIR_0_PLUS;
                    b_dirState = HIGH;
                } else {
                    DIR_0_MINUS;
                    b_dirState = LOW;
                }
                timerAlarmWrite(stepGen_0, ul_dirSetup[0], true);
                b_dirChange[0] = false;
            }
        } else { // nem kell impulzus
            timerAlarmWrite(stepGen_0, 4000ul, true); // 0,1 millis scan
            b_math[0] = true; // kiszámítjuk a következő értékeket
        }
    } else { //priódus közepe step H-L átmenet
        STEP_0_L;
        timerAlarmWrite(stepGen_0, ul_TL[0], true);
        b_stepState = LOW;
    }
}
/*==================================================================*/
void IRAM_ATTR onTime_1()
{
    volatile static bool b_stepState = LOW;
    volatile static bool b_dirState = LOW;

    if (b_stepState == LOW) { // periódus vége
        if (ul_T[1]) { // van periódisidő tehát impulzust kell indítani
            if (!b_dirChange[1]) { //jó az irány, új impulzus indítása step L-H átmenet
                STEP_1_H;
                timerAlarmWrite(stepGen_1, ul_TH[1], true);
                b_stepState = HIGH;
                b_dirState ? fb.pos[1]++ : fb.pos[1]--;
                b_math[1] = true; // kiszámítjuk a következő értékeket
            } else { // irányt kell váltani
                if (b_dirSignal[1]) {
                    DIR_1_PLUS;
                    b_dirState = HIGH;
                } else {
                    DIR_1_MINUS;
                    b_dirState = LOW;
                }
                timerAlarmWrite(stepGen_1, ul_dirSetup[1], true);
                b_dirChange[1] = false;
            }
        } else { // nem kell impulzus
            timerAlarmWrite(stepGen_1, 4000ul, true); // 0,1 millis scan
            b_math[1] = true; // kiszámítjuk a következő értékeket
        }
    } else { //priódus közepe step H-L átmenet
        STEP_1_L;
        timerAlarmWrite(stepGen_1, ul_TL[1], true);
        b_stepState = LOW;
    }
}
/*==================================================================*/
void IRAM_ATTR onTime_2()
{
    volatile static bool b_stepState = LOW;
    volatile static bool b_dirState = LOW;

    if (b_stepState == LOW) { // periódus vége
        if (ul_T[2]) { // van periódisidő tehát impulzust kell indítani
            if (!b_dirChange[2]) { //jó az irány, új impulzus indítása step L-H átmenet
                STEP_2_H;
                timerAlarmWrite(stepGen_2, ul_TH[2], true);
                b_stepState = HIGH;
                b_dirState ? fb.pos[2]++ : fb.pos[2]--;
                b_math[2] = true; // kiszámítjuk a következő értékeket
            } else { // irányt kell váltani
                if (b_dirSignal[2]) {
                    DIR_2_PLUS;
                    b_dirState = HIGH;
                } else {
                    DIR_2_MINUS;
                    b_dirState = LOW;
                }
                timerAlarmWrite(stepGen_2, ul_dirSetup[2], true);
                b_dirChange[2] = false;
            }
        } else { // nem kell impulzus
            timerAlarmWrite(stepGen_2, 4000ul, true); // 0,1 millis scan
            b_math[2] = true; // kiszámítjuk a következő értékeket
        }
    } else { //priódus közepe step H-L átmenet
        STEP_2_L;
        timerAlarmWrite(stepGen_2, ul_TL[2], true);
        b_stepState = LOW;
    }
}
/*==================================================================*/
void IRAM_ATTR newT(int i) //T kiszámítása
{
    ul_T[i] = 40000000.0f / sqrtf((float)(ul_accelStep[i] * ul_maxAcc_x2[i]));
    ul_TH[i] = ul_T[i] >> 1;
    ul_TL[i] = ul_T[i] - ul_TH[i];
}
/*==================================================================*/
void IRAM_ATTR deceleration(int i)
{
    if (ul_accelStep[i]) {
        ul_accelStep[i]--;
        if (ul_accelStep[i])
            newT(i);
        else
            ul_T[i] = 0;
    }
}
/*==================================================================*/
void IRAM_ATTR acceleration(int i)
{
    if (cmd.control & CTRL_ENABLE) {
        if (ul_T[i] > ul_min_T[i] || !ul_T[i]) {
            ul_accelStep[i]++;
            newT(i);
        }
    } else
        deceleration(i);
}
/*==================================================================*/
void IRAM_ATTR outputHandler()
{
    if (cmd.control & CTRL_ENABLE) {
        (cmd.io & IO_00) ? OUT_00_H : OUT_00_L;
        (cmd.io & IO_01) ? OUT_01_H : OUT_01_L;
        (cmd.io & IO_02) ? OUT_02_H : OUT_02_L;
    } else {
        OUT_00_L;
        OUT_01_L;
        OUT_02_L;
    }
}
/*==================================================================*/
void IRAM_ATTR inputHandler()
{
    (IN_00) ? fb.io = IO_00 : fb.io = 0;
    if (IN_01)
        fb.io |= IO_01;
    if (IN_02)
        fb.io |= IO_02;
    if (IN_03)
        fb.io |= IO_03;
}
/*==================================================================*/
void IRAM_ATTR commandHandler()
{
    if (cmd.control & CTRL_READY) {

        for (int i = 0; i < 3; i++) {

            if (cmd.vel[i] > 0.0f)
                ul_cmd_T[i] = (unsigned long)(40000000.0f / cmd.vel[i]);
            else if (cmd.vel[i] < 0.0f)
                ul_cmd_T[i] = (unsigned long)(40000000.0f / -cmd.vel[i]);
            else
                ul_cmd_T[i] = 40000000ul;

            l_cmd_pos_last[i] = l_cmd_pos[i];
            l_cmd_pos[i] = cmd.pos[i];
            l_predictive_pos[i] = l_cmd_pos[i] + l_cmd_pos[i] - l_cmd_pos_last[i];
        }
    }

    if (!(fb.control & CTRL_READY)) {

        if ((fb.control & CTRL_DIRSETUP)
            && (fb.control & CTRL_MAXACC)
            && (fb.control & CTRL_MAXVEL)) {
            fb.control |= CTRL_READY;
            //log_printf("READY\n");

        } else if (cmd.control & CTRL_DIRSETUP) {
            fb.control |= CTRL_DIRSETUP;
            for (int i = 0; i < 3; i++)
                ul_dirSetup[i] = cmd.pos[i] / 25; //   25ns / timer tic

        } else if (cmd.control & CTRL_MAXVEL) {
            fb.control |= CTRL_MAXVEL;
            for (int i = 0; i < 3; i++)
                ul_min_T[i] = 40000000ul / cmd.pos[i];

        } else if (cmd.control & CTRL_MAXACC) {
            fb.control |= CTRL_MAXACC;
            for (int i = 0; i < 3; i++)
                ul_maxAcc_x2[i] = cmd.pos[i] << 1;
        }
    }
}
/*==================================================================*/
void IRAM_ATTR loop_Core0(void* parameter)
{
    for (;;) {

        if (Udp.parsePacket()) { // >= sizeof(cmd) + 1

            Udp.read(packetBuffer, sizeof(cmd) + 1);

            uint8_t chk = 71;
            for (int i = 0; i < sizeof(cmd); i++)
                chk ^= packetBuffer[i];

            if (packetBuffer[sizeof(cmd)] == chk) {
                memcpy(&cmd, &packetBuffer, sizeof(cmd));
                commandHandler();
                ul_watchdog = millis();
            }

            inputHandler();

            for (int i = 0; i < 3; i++) {
                unsigned long ul_t = ul_T[i];
                if (ul_t)
                    b_dirSignal[i] ? fb.vel[i] = 40000000.0f / (float)ul_t : fb.vel[i] = -40000000.0f / (float)ul_t;
                else
                    fb.vel[i] = 0.0f;
            }

            memcpy(&packetBuffer, &fb, sizeof(fb));

            Udp.beginPacket(Udp.remoteIP(), Udp.remotePort());
            Udp.write(packetBuffer, sizeof(fb));
            Udp.endPacket();

            outputHandler();
        }

        if (millis() - ul_watchdog > 10ul) {
            fb.control = 0;
            cmd.control = 0;
            outputHandler();
            ul_watchdog = millis();
        }
    }
}
/*==================================================================*/
void setup_Core0(void* parameter)
{
    pinMode(OUT_00_PIN, OUTPUT);
    digitalWrite(OUT_00_PIN, 0);
    pinMode(OUT_01_PIN, OUTPUT);
    digitalWrite(OUT_01_PIN, 0);
    pinMode(OUT_02_PIN, OUTPUT);
    digitalWrite(OUT_02_PIN, 0);

    pinMode(IN_00_PIN, INPUT_PULLUP);
    pinMode(IN_01_PIN, INPUT_PULLUP);
    pinMode(IN_02_PIN, INPUT_PULLUP);
    pinMode(IN_03_PIN, INPUT_PULLUP);

    //pinMode(IN_04_PIN, INPUT);
    //pinMode(IN_05_PIN, INPUT);
    //pinMode(IN_06_PIN, INPUT);
    //pinMode(IN_07_PIN, INPUT);

    delay(100);
    Ethernet.init(SPI_CS_PIN); /* You can use Ethernet.init(pin) to configure the CS pin */
    Ethernet.begin(mac, ip); /* start the Ethernet */

    delay(100);
    Udp.begin(port); /* start UDP */

    delay(100);
    xTaskCreatePinnedToCore(
        loop_Core0, // Task function.
        "loopTask_Core0", // name of task.
        3000, // Stack size of task
        NULL, // parameter of the task
        0, // priority of the task
        NULL, // Task handle to keep track of created task
        0); // pin task to core 0

    disableCore0WDT();

    vTaskDelete(NULL);
}
/*==================================================================*/
void setup()
{
    pinMode(STEP_0_PIN, OUTPUT);
    STEP_0_L;
    pinMode(DIR_0_PIN, OUTPUT);
    DIR_0_MINUS;
    pinMode(STEP_1_PIN, OUTPUT);
    STEP_1_L;
    pinMode(DIR_1_PIN, OUTPUT);
    DIR_1_MINUS;
    pinMode(STEP_2_PIN, OUTPUT);
    STEP_2_L;
    pinMode(DIR_2_PIN, OUTPUT);
    DIR_2_MINUS;

    // Configure the Prescaler
    // 80 000 000 / 2 = 40 000 000 tics/second ===>   25 ns/tic
    stepGen_0 = timerBegin(0, 2, true);
    stepGen_1 = timerBegin(1, 2, true);
    stepGen_2 = timerBegin(2, 2, true);
    timerAttachInterrupt(stepGen_0, &onTime_0, true);
    timerAttachInterrupt(stepGen_1, &onTime_1, true);
    timerAttachInterrupt(stepGen_2, &onTime_2, true);
    timerAlarmWrite(stepGen_0, 40000000, true);
    timerAlarmWrite(stepGen_1, 40000000, true);
    timerAlarmWrite(stepGen_2, 40000000, true);
    timerAlarmEnable(stepGen_0);
    timerAlarmEnable(stepGen_1);
    timerAlarmEnable(stepGen_2);

    xTaskCreatePinnedToCore(
        setup_Core0, // Task function.
        "setup_Core0Task", // name of task.
        1000, // Stack size of task
        NULL, // parameter of the task
        0, // priority of the task
        NULL, // Task handle to keep track of created task
        0); // pin task to core 0
}
/*==================================================================*/
void IRAM_ATTR loop()
{
    for (int i = 0; i < 3; i++) {
        if (b_math[i]) {
            b_math[i] = false;
            if (ul_accelStep[i] == 0) { // áll a motor
                long l_pos_error = l_predictive_pos[i] - fb.pos[i];
                if (l_pos_error != 0) { // ha van hiba
                    if ((l_pos_error > 0 && b_dirSignal[i] == HIGH) || (l_pos_error < 0 && b_dirSignal[i] == LOW)) // jó az irány
                        acceleration(i);
                    else { // irányt kell váltani
                        (l_pos_error > 0) ? b_dirSignal[i] = HIGH : b_dirSignal[i] = LOW;
                        b_dirChange[i] = true;
                    }
                }
            } else { // mozgásban van a motor
                if ((cmd.vel[i] > 0.0f && b_dirSignal[i] == HIGH) || (cmd.vel[i] < 0.0f && b_dirSignal[i] == LOW)) { // jó az irány
                    if (ul_T[i] > ul_cmd_T[i]) // kicsi a sebesség
                        acceleration(i);
                    else if (ul_T[i] < ul_cmd_T[i]) // nagy a sebesség
                        deceleration(i);
                } else // rossz az irány vagy 0 a célsebesség
                    deceleration(i);
            }
        }
    }
}