#include <Arduino.h>

#include "motor.h"
#include "integrator.h"
#include "PID.h"
#include "Controller.h"
#include "display.h"

Rotax915iS motor(600.0);
Rotax915iS motor_nn(600.0);
Integrator throttle(1.5);
Integrator throttle_nn(1.5);
PID pid;
CONTROLLER controller;

float fordulatszam = 0.0f;
float fordulatszam_nn = 0.0f;

static void IRAM_ATTR pid_task(void* arg)
{
  Serial.begin(115200);
  //for (int i = 0; i < 1000; i++)
  //  Serial.println("0.0");

  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    xTaskDelayUntil(&xLastWakeTime, 100);

    static float teher = 2.0; //2.625; //Nm

    static float thr, thr_nn;

    static float z = 0;
    static bool p = 0;

    if (digitalRead(14) == LOW)
      controller.print();

    if (digitalRead(4) == LOW) {
      /*static double x = 0;
        x += .2;
        double s = sin(x);
        teher = 140 + s * 20;*/
      float cc = random(0, 20);
      teher += random(-cc, cc) + z;
    }
    if (digitalRead(17) == LOW)
      teher += 10;
    if (digitalRead(18) == LOW)
      teher -= 10;

    if (teher < 2.0)
      teher = 2.0;
    if (teher > 160)
      teher = 160;

    if (z < 1.0) {
      if (p == true)
        z += 0.01;
    } else p = false;
    if (z > 0.0) {
      if (p == false)
        z -= 0.01;
    } else p = true;

    float governor = 0;
    float governor_nn = 0;
    float error = 1.0 - fordulatszam / 5500.0;
    float error_nn = 1.0 - fordulatszam_nn / 5500.0;

    governor = pid.update(error);

    if (digitalRead(10) == LOW) {
      controller.setMode(PID_TRAINING);
      governor_nn = controller.update(error, governor);
    } else {
      controller.setMode(SELF_LEARNING);
      governor_nn = controller.update(error_nn);
    }

    thr = throttle.update(governor);

    if (digitalRead(10) == LOW) {
      thr_nn = throttle_nn.update(governor);
    } else
      thr_nn = throttle_nn.update(governor_nn);

    if (thr < 0.39)
      thr = 0.39;
    if (thr_nn < 0.39)
      thr_nn = 0.39;

    fordulatszam = motor.rpm(thr * 100.0, teher);
    fordulatszam_nn = motor_nn.rpm(thr_nn * 100.0, teher);

    Serial.print( (fordulatszam / 5500 - 1) * 10 + 1.2, 2);
    Serial.print( "," );
    Serial.print( (fordulatszam_nn / 5500 - 1) * 10 + 1.2, 2);
    Serial.print( "," );
    Serial.print( thr, 2);
    Serial.print( "," );
    Serial.print( governor, 2);
    Serial.print( "," );
    Serial.print( governor_nn, 2);
    Serial.print( "," );
    Serial.print( thr_nn, 2);
    Serial.print( "," );
    Serial.print( teher / 170, 2);
    Serial.print( "," );
    Serial.print( "1.2");
    Serial.print( "," );
    Serial.println( w[10]* 10 + 1.2, 2);
  }
}


void setup() {
  setCpuFrequencyMhz(240);

  pid.set_params(6.5, 0, 1.23);

  pinMode(4, INPUT_PULLUP);
  pinMode(17, INPUT_PULLUP);
  pinMode(18, INPUT_PULLUP);
  pinMode(10, INPUT_PULLUP);
  pinMode(14, INPUT_PULLUP);

  delay(1000);

  xTaskCreatePinnedToCore(pid_task, "pid_task", 65536, NULL, 2, NULL, 1);
  xTaskCreatePinnedToCore(display_task, "display_task", 4096, NULL, 0, NULL, 0);

  vTaskDelete(NULL);
}

void loop() {}


sketch.ino

diagram.json

#pragma once

#include <Arduino.h>

// Aktiváló függvények
class ActivationFunction {
public:
    virtual float calculate(float x) = 0;
    virtual float derivative(float x) = 0;
};

class Sigmoid : public ActivationFunction {
public:
    static Sigmoid& getInstance()
    {
        static Sigmoid instance; // Egyetlen példány
        return instance;
    }
    float calculate(float x) override { return 1.0 / (1.0 + exp(-x)); }
    float derivative(float x) override
    {
        float sigmoid_x = 1.0 / (1.0 + exp(-x));
        return sigmoid_x * (1.0 - sigmoid_x);
    }
};

class Tanh : public ActivationFunction {
public:
    static Tanh& getInstance()
    {
        static Tanh instance; // Egyetlen példány
        return instance;
    }
    float calculate(float x) override { return tanh(x); }
    float derivative(float x) override
    {
        float tanh_x = tanh(x);
        return 1.0 - tanh_x * tanh_x;
    }
};

class ReLU : public ActivationFunction {
public:
    static ReLU& getInstance()
    {
        static ReLU instance; // Egyetlen példány
        return instance;
    }
    float calculate(float x) override { return (x > 0) ? x : 0; }
    float derivative(float x) override { return (x > 0) ? 1 : 0; }
};

class Linear : public ActivationFunction {
public:
    static Linear& getInstance()
    {
        static Linear instance; // Egyetlen példány
        return instance;
    }
    float calculate(float x) override { return x; }
    float derivative(float) override { return 1.0; }
};

class LeakyReLU : public ActivationFunction {
public:
    static LeakyReLU& getInstance()
    {
        static LeakyReLU instance; // Singleton instance
        return instance;
    }
    float calculate(float x) override { return (x > 0) ? x : 0.01 * x; }
    float derivative(float x) override { return (x > 0) ? 1 : 0.01; }
};

class Softmax : public ActivationFunction {
public:
    static Softmax& getInstance()
    {
        static Softmax instance; // Singleton instance
        return instance;
    }
    float calculate(float x) override {
        // Softmax is typically applied to a whole layer, so this method
        // can be adapted as part of a neural network forward pass to normalize values
        return x; // Placeholder for the actual softmax logic in the full network context
    }
    float derivative(float) override {
        // Softmax derivatives require the entire output vector
        return 1.0; // Placeholder
    }
};

class Swish : public ActivationFunction {
public:
    static Swish& getInstance()
    {
        static Swish instance; // Singleton instance
        return instance;
    }
    float calculate(float x) override { return x / (1.0 + exp(-x)); }
    float derivative(float x) override
    {
        float swish_x = x / (1.0 + exp(-x));
        return swish_x + (1.0 - swish_x) * (1.0 / (1.0 + exp(-x)));
    }
};

ActivationFunction.h

#pragma once

#include <Arduino.h>

#include "NN_Optimizer.h"
#include "NeuralNetwork.h"

float w[100] = {};

enum Mode {
  PID_TRAINING,
  SELF_LEARNING,
  USE_TRAINED
};

class CONTROLLER {
  private:
    nn_struct_t* nn;
    optimizer_struct_t* optimizer;

    NeuralNetwork* network;
    Optimizer* trainer;

    Mode mode;
    float last_process_error = 0;
    float last_output = 0;
    float last_target = 0;
    float learning_rate;
    float momentum;

  public:
    CONTROLLER()
      : mode(PID_TRAINING)
    {
      nn = new nn_struct_t;
      nn->bias_disable = true;
      nn->num_layers = 3;
      nn->topology = new int[nn->num_layers] { 20, 20, 1 }; // { 20, 6, 1 };
      nn->activation_type = new ActivationType[layers] { LINEAR, TANH, TANH };  // Az egyes rétegek aktivációs függvényei

      optimizer = new optimizer_struct_t;
      optimizer->learning_rate = 0.03;
      optimizer->learning_rate_of_bias = 0.003;

      network = new NeuralNetwork(nn);

      //trainer = new SGDoptimizer(nn, optimizer);
      //trainer = new MomentumOptimizer(nn, optimizer);
      //trainer = new NesterovMomentumOptimizer(nn, optimizer);
      trainer = new AdamOptimizer(nn, optimizer);

      network->initialize();
    }

    ~CONTROLLER()
    {
      delete trainer;
      delete network;
      delete[] nn->activation_type;
      delete[] nn->topology;
      delete nn;
      delete optimizer;
    }

    void print()
    {
      network->print();
    }

    void setMode(const Mode new_mode)
    {
      mode = new_mode;
    }

    void clipping(float& input, const float threshold)
    {
      if (input > threshold)
        input = threshold;
      else if (input < -threshold)
        input = -threshold;
    }

    int cou = 0;

    float update(float process_error, const float target = 0)
    {

      //-process_error *= -1.0; //////////////////////////////

      if (cou < 1000)
        cou++;

      if (mode == SELF_LEARNING && cou > 80) {

        error(0) = -process_error; // hiba

        if (process_error * last_process_error < 0) // túllendülés
          error(0) = last_process_error * 1.1;
        //else if (abs(process_error) > abs(last_process_error)) // növekszik a hiba - ok
         // error(0) *= 1.1;
        //else if (abs(process_error) < abs(last_process_error)) // csökken a hiba - ok
          //error(0) -= (process_error - last_process_error);

        //if (output(0) * last_output < 0) // aktuátor vibráció
        //error(0) *= 10.0;
        //error(0) += (output(0) - last_output); // nagy aktuátor jel különbség - ok

        w[10] = error(0);

        clipping(error(0), 1.0);
        // if (fabs(error(0)) < 0.3) //////////// 0.05 /////////////////
        trainer->update();
      }

      for (int i = input_neurons - 1; i > 0; --i) // inputok frissítése
        input(i) = input(i - 1);
      input(10) = (process_error - last_process_error) * 100.0;
      input(0) = process_error * 10.0;

      /*for (int i = 0; i < input_neurons; ++i) // inputok frissítése
        Serial.println(input(i));
        delay(10000);*/

      clipping(input(0), 1.0);
      //clipping(input(10), 1.0);

      network->forward();

      if (mode == PID_TRAINING) {
        error(0) = -(target - output(0));
        // if (fabs(error(0)) < 0.4)
        if (cou > 80)
          trainer->update();
      }

      // clipping(output[0], 1.0); // A kimenet korlátozása a megfelelő tartományba

      last_output = output(0);
      last_process_error = process_error;
      last_target = target;

      // for (int i = 0; i < number_of_neurons_in_input_layer; --i)
      //   w[i] = nn->weights[0][0][i];
      /*w[0] = optimizer->delta[0][0];
        w[1] = optimizer->delta[0][1];
        w[2] = optimizer->delta[1][0];
        w[3] = optimizer->delta[1][1];
        w[4] = optimizer->delta[2][0];
        w[5] = optimizer->delta[2][1];
        w[6] = optimizer->delta[3][0];
        w[7] = optimizer->delta[3][1];*/

      w[0] = nn->weight[0][0][0];
      w[1] = nn->weight[0][0][1];
      w[2] = nn->weight[0][0][2];
      w[3] = nn->weight[0][0][3];
      w[4] = nn->weight[0][0][4];
      w[5] = nn->weight[0][0][5];
      w[6] = nn->weight[0][0][6];
      w[7] = nn->weight[0][0][7];

      return last_output;
    }
};
/*

  -2.130658, -1.556214, 0.141378, 0.070159, 0.456576, -0.071552, -0.147351, -0.855464, -0.075816, 0.785060, -5.635824, -1.816217, -0.505459, 0.362612, -0.384781, 0.490385, 0.179062, -0.777602, -0.295994, -0.434019,
  -2.256881, -0.938867, -1.963806, -0.386738, -1.462066, -0.649425, 0.371475, 0.156069, 0.113967, 1.418798, -1.624732, -1.306958, -2.006584, -1.657267, -1.309381, -0.659795, -0.599722, 0.626921, 0.625838, 0.255462,
  -0.744042, -0.254351, 1.216622, -0.757097, -1.029083, -0.056519, -0.323492, -0.407178, -0.811973, 0.766409, -2.580098, 0.429338, 0.971854, 0.819933, 0.457585, 0.783583, 1.928982, -0.303019, 1.258433, -0.614299,
  1.846881, 0.575032, 0.941716, -0.163150, -0.051951, 0.761525, 0.368157, -0.615915, -0.751517, 0.129173, 6.732378, 0.963960, 0.007232, 1.065922, 0.147955, -0.060020, 0.737615, -0.125926, 0.921241, 0.695845,
  0.943761, 1.306579, 0.406371, 0.936437, 1.525507, 1.220173, -0.459657, -0.833567, -0.130127, -1.337744, 1.716351, 0.195972, 0.726973, 1.318263, 0.321744, 1.064981, 1.440261, 0.942598, 1.275794, 1.680254,
  -0.290261, -0.170623, -0.213999, -1.209895, -0.046167, -0.546149, -0.791353, -0.201963, -1.301572, -2.242276, -0.680421, -0.542864, -0.245725, 0.156483, 0.605884, -0.102438, 0.839226, 1.534781, 1.496667, 2.481053,
  -0.609689, -0.105349, -0.200432, 0.521630, 0.169654, -0.292432,

  0.620552, 0.259336, -0.611114, -0.426449, -0.143123, 0.055923, 0.492411,

*/

Controller.h


// Display samples of fonts.
//
#include <Wire.h>
#include "SSD1306Ascii.h"
#include "SSD1306AsciiWire.h"

// 0X3C+SA0 - 0x3C or 0x3D
#define I2C_ADDRESS 0x3C

// Define proper RST_PIN if required.
#define RST_PIN -1

const char* fontName[] = {
  "Arial10",
  "Arial_bold_14",
  "Callibri11",
  "Callibri11_bold",
  "Callibri11_italic",
  "Callibri13",
  "Corsiva_12",
  "fixed_bold10x15",
  "font5x7",
  "font8x8",
  "Iain5x7",
  "lcd5x7",
  "Stang5x7",
  "System5x7",
  "TimesNewRoman16",
  "TimesNewRoman16_bold",
  "TimesNewRoman16_italic",
  "utf8font10x16",
  "Verdana12",
  "Verdana12_bold",
  "Verdana12_italic",
  "X11fixed7x14",
  "X11fixed7x14B",
  "ZevvPeep8x16"
};
const uint8_t* fontList[] = {
  Arial14,
  Arial_bold_14,
  Callibri11,
  Callibri11_bold,
  Callibri11_italic,
  Callibri15,
  Corsiva_12,
  fixed_bold10x15,
  font5x7,
  font8x8,
  Iain5x7,
  lcd5x7,
  Stang5x7,
  System5x7,
  TimesNewRoman16,
  TimesNewRoman16_bold,
  TimesNewRoman16_italic,
  utf8font10x16,
  Verdana12,
  Verdana12_bold,
  Verdana12_italic,
  X11fixed7x14,
  X11fixed7x14B,
  ZevvPeep8x16
};
uint8_t nFont = sizeof(fontList) / sizeof(uint8_t*);

SSD1306AsciiWire oled;
//------------------------------------------------------------------------------

static void IRAM_ATTR display_task(void* arg)
{
  Wire.begin();
  Wire.setClock(400000L);
  oled.begin(&Adafruit128x64, I2C_ADDRESS);
  oled.setFont(System5x7);
  oled.setFont(lcd5x7);

  TickType_t xLastWakeTime = xTaskGetTickCount();

  for (;;) {
    xTaskDelayUntil(&xLastWakeTime, 1000);

    oled.clear();
    oled.println(w[0], 4);
    oled.println(w[1], 4);
    //oled.println();
    oled.println(w[2], 4);
    oled.println(w[3], 4);
    oled.println(w[4], 4);
    oled.println(w[5], 4);
    //oled.println();
    oled.println(w[6],4);
    oled.println(w[7],4);

  }
}


display.h

#include <Arduino.h>

class Integrator {
  private:

    float timeConst;
    float output = 0;
    uint32_t last_time = 0;

  public:
    Integrator(float time): timeConst(time) {}

    float update(float input) {

      uint32_t time = micros();

      //if (abs(input) < 0.2) input = 0;

      if (last_time != 0) {
        float dt = (float)(time - last_time) / 1000000.0; // sec
        output += input * dt / timeConst ;
      }

      if (output > 1.0)
        output = 1.0;
      else if (output < 0.0)
        output = 0.0;

      last_time = time;

      return output;
    }
};

integrator.h

# Wokwi Library List
# See https://docs.wokwi.com/guides/libraries


SSD1306Ascii


libraries.txt

#include <Arduino.h>

class Rotax915iS { // Rotax 915iS motor osztály

  private:
    float n; // Fordulatszám 1/s
    float J; // Tehetetlenségi nyomaték
    uint32_t last_time;

    float torque(const float n, const float t) {
      return n * t * (n * t * (t * (n * -3.77016584231204E-08 + 8.39082865772136E-06) + n * 8.18233754720831E-06 - 0.0018288017364816) + n * (n * -0.000571956163730708 + 0.127579197577619) + t * (t * -0.000550696563294183 + 0.121931452025011) - 8.54763642710975) + t * (t * (t * 0.0113209942154886 - 2.57764985274687) + 185.026633304969) + n * (n * (n * 0.0129967529467918 - 2.88056297845111) + 191.746134591097) - 4160.48201418587;
    }

  public:
    Rotax915iS(float J): J(J) {
      n = 33;
      last_time = 0;
    }

    float rpm(float throttle, float load) { // % , Nm

      uint32_t time = millis();

      if (last_time) {
        uint32_t dt = time - last_time;

        float torqueDifference = torque(n, throttle) - load;
        float beta = torqueDifference / J; // szöggyorsulás
        float delta_omega = beta * dt; // szögsebesség változás
        float delta_n = delta_omega / (2 * PI); // fordulatszám változás

        //float dRPM_dt = torqueDifference / J; // Fordulatszám-változás
        //RPM += dRPM_dt; // Fordulatszám frissítése

        n += delta_n;

        // limitek
        if (n < 33) n = 33;
        if (n > 100) n = 100;
      }

      last_time = time;

      return n * 60;
    }
};

motor.h

#pragma once

#include <Arduino.h>

#include "esp_dsp.h"

#include "NN_defs.h"

#include "ActivationFunction.h"

class NeuralNetwork {
private:
    nn_struct_t* nn;

public:
    NeuralNetwork(nn_struct_t* nn_struct)
        : nn(nn_struct)
    {
        nn->weight = new float**[layers - 1];
        if (nn->bias_disable == false)
            nn->bias = new float*[layers - 1];
        nn->activation = new float*[layers];
        nn->activation_function = new ActivationFunction*[layers]; // One per layer (no except input)

        for (int i = 0; i < layers; ++i) {
            if (i < layers - 1) {
                nn->weight[i] = new float*[nn->topology[i + 1]];
                for (int j = 0; j < nn->topology[i + 1]; ++j)
                    nn->weight[i][j] = new float[nn->topology[i]]();
                if (nn->bias_disable == false)
                    nn->bias[i] = new float[nn->topology[i + 1]]();
            }
            nn->activation[i] = new float[nn->topology[i]]();
            // Assign activation function based on enum
            switch (nn->activation_type[i]) {
            case TANH:
                // activation_functions[i] = new Tanh();
                nn->activation_function[i] = &Tanh::getInstance(); // Use Singleton instance
                break;
            case RELU:
                nn->activation_function[i] = &ReLU::getInstance();
                break;
            case SIGMOID:
                nn->activation_function[i] = &Sigmoid::getInstance();
                break;
            case LEAKYRELU:
                nn->activation_function[i] = &LeakyReLU::getInstance();
                break;
            case LINEAR:
                nn->activation_function[i] = &Linear::getInstance();
                break;
            }
        }
    }

    ~NeuralNetwork()
    {
        // Cleanup memory
        for (int i = 0; i < layers; ++i) {
            if (i < layers - 1) {
                for (int j = 0; j < nn->topology[i + 1]; ++j)
                    delete[] nn->weight[i][j];
                delete[] nn->weight[i];
                if (nn->bias_disable == false)
                    delete[] nn->bias[i];
            }
            delete[] nn->activation[i];
            // Don't delete activation_functions[layer], since they are Singleton instances
        }

        delete[] nn->weight;
        if (nn->bias_disable == false)
            delete[] nn->bias;
        delete[] nn->activation;
        delete[] nn->activation_function; // This can be deleted, but not individual elements
    }

    void initialize()
    {
        for (int i = 0; i < layers - 1; ++i) {

            for (int j = 0; j < nn->topology[i]; ++j)
                nn->activation[i][j] = (float)random(-100, 100) / 100.0; // Random value;

            for (int j = 0; j < nn->topology[i + 1]; ++j) {
                if (nn->bias_disable == false)
                    nn->bias[i][j] = (float)random(-100, 100) / 100.0; // Random value
                for (int k = 0; k < nn->topology[i]; ++k)
                    nn->weight[i][j][k] = (float)random(-100, 100) / 100.0; // Random value
            }
        }
        nn->weight[0][0][0]=08.93;
        nn->weight[0][0][1]=-08.26;
    }

    // Forward propagation
    void forward()
    {
        //forward_SIMD();
        //return;

        for (int layer = 1; layer < layers; ++layer)
            for (int neuron = 0; neuron < neurons; ++neuron) {
                nn->activation[layer][neuron] = 0;
                for (int prev_neuron = 0; prev_neuron < prev_neurons; ++prev_neuron)
                    nn->activation[layer][neuron] += nn->weight[layer - 1][neuron][prev_neuron] * nn->activation[prev_layer][prev_neuron];
                if (nn->bias_disable == false)
                    nn->activation[layer][neuron] += nn->bias[layer - 1][neuron];
                nn->activation[layer][neuron] = nn->activation_function[layer]->calculate(nn->activation[layer][neuron]);
            }
    }

    // Forward propagation SIMD optimalizációval
    void forward_SIMD()
    {
        for (int layer = 1; layer < layers; ++layer) {
            // SIMD optimalizált dot product az előző réteg aktivációi és a súlyok között
            for (int neuron = 0; neuron < neurons; ++neuron) {
                nn->activation[layer][neuron] = 0;
                dsps_dotprod_f32(nn->activation[prev_layer], nn->weight[layer - 1][neuron], &nn->activation[layer][neuron], prev_neurons);
            }
            // Az aktuális bias érték hozzáadása
            if (nn->bias_disable == false)
                dsps_add_f32(nn->activation[layer], nn->bias[layer - 1], nn->activation[layer], neurons, 1, 1, 1);
            // Aktivációs függvény alkalmazása
            for (int neuron = 0; neuron < neurons; ++neuron)
                nn->activation[layer][neuron] = nn->activation_function[layer]->calculate(nn->activation[layer][neuron]);
        }
    }

    void print()
    {
        delay(1000);

        Serial.println();
        Serial.println();

        for (int i = 0; i < layers - 1; ++i)
            for (int j = 0; j < nn->topology[i + 1]; ++j)
                for (int k = 0; k < nn->topology[i]; ++k) {
                    Serial.print(nn->weight[i][j][k], 6);
                    Serial.print(", ");
                    delay(10);
                }

        Serial.println();
        Serial.println();

        // if (nn->bias_disable == false)
        for (int i = 0; i < layers - 1; ++i)
            for (int j = 0; j < nn->topology[i + 1]; ++j) {
                Serial.print(nn->bias[i][j], 6);
                Serial.print(", ");
                delay(10);
            }

        Serial.println();
        Serial.println();

        delay(5000);
    }
};


NeuralNetwork.h

#pragma once

#define layers nn->num_layers
#define input_layer 0
#define output_layer layers - 1
#define next_layer layer + 1
#define prev_layer layer - 1

#define neurons nn->topology[layer]
#define prev_neurons nn->topology[layer - 1]
#define next_neurons nn->topology[layer + 1]
#define output_neurons nn->topology[output_layer]
#define input_neurons nn->topology[0]

#define error(num) optimizer->delta[output_layer - 1][num]
#define output(num) nn->activation[output_layer][num]
#define input(num) nn->activation[input_layer][num]

class ActivationFunction;
// class LossFunction;

enum ActivationType {
    SIGMOID,
    TANH,
    RELU,
    LINEAR,
    LEAKYRELU,
    SOFTMAX,
    SWISH
};

typedef struct nn_struct_t {
    int num_layers; // A rétegek száma az input réteggel együtt
    int* topology; // Rétegekben lévő neuronok száma
    ActivationType* activation_type;
    float*** weight; // 3D array: [layer][neuron_in_layer][neuron_in_previous_layer]
    float** bias; // 2D array: [layer][neuron_in_layer]
    float** activation; // 2D array: [layer][neuron_in_layer]
    ActivationFunction** activation_function; // Activation functions per layer
    bool bias_disable;
};

typedef struct optimizer_struct_t {
    float learning_rate;
    float learning_rate_of_bias;
    float** delta; // Delták a backpropagation-hoz [layer][neuron_in_layer]
    // LossFunction* loss_function;
};


NN_defs.h

#pragma once

#include <Arduino.h>

#include "esp_dsp.h"

#include "NN_defs.h"

#include "ActivationFunction.h"

class Optimizer {
protected:
    nn_struct_t* nn;
    optimizer_struct_t* me;
    float* temp_array;

public:
    // Konstruktor a közös adatok inicializálásához
    Optimizer(nn_struct_t* nn_struct, optimizer_struct_t* optimizer_struct)
        : nn(nn_struct)
        , me(optimizer_struct)
    {
        me->delta = new float*[layers - 1];
        for (int i = 0; i < layers - 1; ++i)
            me->delta[i] = new float[nn->topology[i + 1]]();

        int neurons_max = 0;
        for (int i = 0; i < layers; ++i)
            if (nn->topology[i] > neurons_max)
                neurons_max = nn->topology[i];
        temp_array = new float[neurons_max];
    }

    ~Optimizer()
    {
        for (int i = 0; i < layers - 1; ++i)
            delete[] me->delta[i];
        delete[] me->delta;
        delete[] temp_array;
    }

    // Virtuális függvény a gradiens számításhoz (konkrét implementáció a származtatott osztályban)
    virtual void calculate_gradients()
    {
        calculate_gradients_SIMD();
        return;

        for (int i = 0; i < layers - 2; ++i) // a kimenetit nem szabad nullázni
            for (int j = 0; j < nn->topology[i + 1]; ++j)
                me->delta[i][j] = 0;

        for (int layer = output_layer; layer > 0; --layer)
            for (int neuron = 0; neuron < neurons; ++neuron) {
                me->delta[layer - 1][neuron] = me->delta[layer - 1][neuron] * nn->activation_function[layer]->derivative(nn->activation[layer][neuron]);
                // nn->bias[layer - 1][neuron] -= me->delta[layer - 1][neuron] * me->learning_rate;
                if (layer > 1) {
                    for (int prev_neuron = 0; prev_neuron < prev_neurons; prev_neuron++) {
                        me->delta[prev_layer - 1][prev_neuron] += me->delta[layer - 1][neuron] * nn->weight[layer - 1][neuron][prev_neuron];
                        // nn->weight[layer - 1][neuron][prev_neuron] -= me->delta[layer - 1][neuron] * nn->activation[prev_layer][prev_neuron] * me->learning_rate;
                    }
                }
            }
    }

    // Virtuális függvény a gradiens számításhoz (konkrét implementáció a származtatott osztályban)
    virtual void calculate_gradients_SIMD()
    {
        for (int i = 0; i < layers - 2; ++i) // a kimenetit nem szabad nullázni
            memset(me->delta[i], 0, nn->topology[i + 1] * sizeof(float));

        for (int layer = output_layer; layer > 0; --layer)
            for (int neuron = 0; neuron < neurons; ++neuron) {
                me->delta[layer - 1][neuron] = me->delta[layer - 1][neuron] * nn->activation_function[layer]->derivative(nn->activation[layer][neuron]);
                if (layer > 1) {
                    dsps_mulc_f32(nn->weight[layer - 1][neuron], temp_array, prev_neurons, me->delta[layer - 1][neuron], 1, 1);
                    dsps_add_f32(me->delta[prev_layer - 1], temp_array, me->delta[prev_layer - 1], prev_neurons, 1, 1, 1);
                }
            }
    }

    // Tiszta virtuális függvény a súlyok és biasok frissítésére (minden optimalizálónak implementálnia kell)
    virtual void update() = 0;
};

class SGDoptimizer : public Optimizer {
public:
    // Konstruktor az optimalizáló inicializálására
    SGDoptimizer(nn_struct_t* nn_struct, optimizer_struct_t* optimizer_struct)
        : Optimizer(nn_struct, optimizer_struct)
    {
    }
    ~SGDoptimizer() { }

    void update() override
    {
        calculate_gradients();

        for (int layer = 1; layer < layers; ++layer) {
            for (int neuron = 0; neuron < neurons; ++neuron) {
                if (nn->bias_disable == false)
                    nn->bias[layer - 1][neuron] -= me->delta[layer - 1][neuron] * me->learning_rate_of_bias;
                for (int prev_neuron = 0; prev_neuron < prev_neurons; ++prev_neuron)
                    nn->weight[layer - 1][neuron][prev_neuron] -= me->delta[layer - 1][neuron] * nn->activation[prev_layer][prev_neuron] * me->learning_rate;
            }
        }
    }
};

class MomentumOptimizer : public Optimizer {
private:
    float*** velocity_weight; // A súlyokhoz tartozó sebességek (momentum)
    float** velocity_bias; // A biasokhoz tartozó sebességek (momentum)
    float momentum; // Momentum értéke

public:
    // Konstruktor az optimalizáló inicializálására
    MomentumOptimizer(nn_struct_t* nn_struct, optimizer_struct_t* optimizer_struct, float momentum = 0.9)
        : Optimizer(nn_struct, optimizer_struct)
        , momentum(momentum)
    {
        // Inicializálni kell a sebesség tömböket
        velocity_weight = new float**[layers - 1];
        if (nn->bias_disable == false)
            velocity_bias = new float*[layers - 1];
        for (int i = 0; i < layers - 1; ++i) {
            velocity_weight[i] = new float*[nn->topology[i + 1]];
            if (nn->bias_disable == false)
                velocity_bias[i] = new float[nn->topology[i + 1]]();
            for (int j = 0; j < nn->topology[i + 1]; ++j)
                velocity_weight[i][j] = new float[nn->topology[i]]();
        }
    }

    // Destruktor a dinamikus memória felszabadítására
    ~MomentumOptimizer()
    {
        for (int i = 0; i < layers - 1; ++i) {
            if (nn->bias_disable == false)
                delete[] velocity_bias[i];
            for (int j = 0; j < nn->topology[i + 1]; ++j)
                delete[] velocity_weight[i][j];
            delete[] velocity_weight[i];
        }
        delete[] velocity_weight;
        if (nn->bias_disable == false)
            delete[] velocity_bias;
    }

    // Update függvény, amely tartalmazza a momentumot
    void update() override
    {
        calculate_gradients();

        for (int layer = 1; layer < layers; ++layer) {
            for (int neuron = 0; neuron < neurons; ++neuron) {
                if (nn->bias_disable == false) {
                    // Bias frissítése momentum felhasználásával
                    velocity_bias[layer - 1][neuron] *= momentum;
                    velocity_bias[layer - 1][neuron] -= me->delta[layer - 1][neuron] * me->learning_rate_of_bias;
                    nn->bias[layer - 1][neuron] += velocity_bias[layer - 1][neuron]; // Bias frissítése
                }
                // Súlyok frissítése momentum felhasználásával
                for (int prev_neuron = 0; prev_neuron < prev_neurons; ++prev_neuron) {
                    velocity_weight[layer - 1][neuron][prev_neuron] *= momentum;
                    velocity_weight[layer - 1][neuron][prev_neuron] -= me->delta[layer - 1][neuron] * nn->activation[prev_layer][prev_neuron] * me->learning_rate;
                    nn->weight[layer - 1][neuron][prev_neuron] += velocity_weight[layer - 1][neuron][prev_neuron]; // Súly frissítése
                }
            }
        }
    }
};

class NesterovMomentumOptimizer : public Optimizer {
private:
    float*** velocity_weight;
    float** velocity_bias;
    float momentum;

public:
    NesterovMomentumOptimizer(nn_struct_t* nn_struct, optimizer_struct_t* optimizer_struct, float momentum = 0.9)
        : Optimizer(nn_struct, optimizer_struct)
        , momentum(momentum)
    {
        // Initialize velocity for each layer and neuron
        velocity_weight = new float**[layers - 1];
        if (nn->bias_disable == false)
            velocity_bias = new float*[layers - 1];
        for (int i = 0; i < layers - 1; ++i) {
            velocity_weight[i] = new float*[nn->topology[i + 1]];
            for (int j = 0; j < nn->topology[i + 1]; ++j)
                velocity_weight[i][j] = new float[nn->topology[i]]();
            if (nn->bias_disable == false)
                velocity_bias[i] = new float[nn->topology[i + 1]]();
        }
    }

    ~NesterovMomentumOptimizer()
    {
        for (int i = 0; i < layers - 1; ++i) {
            for (int j = 0; j < nn->topology[i + 1]; ++j)
                delete[] velocity_weight[i][j];
            delete[] velocity_weight[i];
            if (nn->bias_disable == false)
                delete[] velocity_bias[i];
        }
        delete[] velocity_weight;
        if (nn->bias_disable == false)
            delete[] velocity_bias;
    }

    void update() override
    {
        calculate_gradients();

        for (int layer = 1; layer < layers; ++layer) {
            for (int neuron = 0; neuron < neurons; ++neuron) {
                for (int prev_neuron = 0; prev_neuron < prev_neurons; ++prev_neuron) {
                    // Compute Nesterov Momentum update
                    float prev_velocity = velocity_weight[layer - 1][neuron][prev_neuron];
                    // Update velocity
                    velocity_weight[layer - 1][neuron][prev_neuron] *= momentum;
                    velocity_weight[layer - 1][neuron][prev_neuron] -= me->delta[layer - 1][neuron] * nn->activation[prev_layer][prev_neuron] * me->learning_rate;

                    // Lookahead step with updated velocity
                    nn->weight[layer - 1][neuron][prev_neuron] += -momentum * prev_velocity + (1 + momentum) * velocity_weight[layer - 1][neuron][prev_neuron];
                }
                if (nn->bias_disable == false) {
                    // Update biases with momentum
                    // Compute Nesterov Momentum update
                    float prev_velocity = velocity_bias[layer - 1][neuron];
                    // Update velocity
                    velocity_bias[layer - 1][neuron] *= momentum;
                    velocity_bias[layer - 1][neuron] -= me->delta[layer - 1][neuron] * me->learning_rate_of_bias;

                    // Lookahead step with updated velocity
                    nn->bias[layer - 1][neuron] += -momentum * prev_velocity + (1 + momentum) * velocity_bias[layer - 1][neuron];
                }
            }
        }
    }
};

class AdamOptimizer : public Optimizer {
private:
    // Adam-specific internal variables for first and second moment estimates
    float*** m_weight;
    float*** v_weight;
    float** m_bias;
    float** v_bias;
    float beta1;
    float beta2;
    float epsilon;
    int t; // Iteration counter

public:
    // Constructor for Adam optimizer initialization
    AdamOptimizer(
        nn_struct_t* nn_struct,
        optimizer_struct_t* optimizer_struct,
        float beta1 = 0.9,
        float beta2 = 0.999,
        float epsilon = 1e-6 // 1e-8
        )
        : Optimizer(nn_struct, optimizer_struct)
        , beta1(beta1)
        , beta2(beta2)
        , epsilon(epsilon)
        , t(0)
    {
        // Initialize Adam-specific first moment (m) and second moment (v) for weights and biases
        m_weight = new float**[layers - 1];
        v_weight = new float**[layers - 1];
        if (nn->bias_disable == false) {
            m_bias = new float*[layers - 1];
            v_bias = new float*[layers - 1];
        }

        for (int i = 0; i < layers - 1; ++i) {
            m_weight[i] = new float*[nn->topology[i + 1]];
            v_weight[i] = new float*[nn->topology[i + 1]];
            if (nn->bias_disable == false) {
                m_bias[i] = new float[nn->topology[i + 1]]();
                v_bias[i] = new float[nn->topology[i + 1]]();
            }
            for (int j = 0; j < nn->topology[i + 1]; ++j) {
                m_weight[i][j] = new float[nn->topology[i]]();
                v_weight[i][j] = new float[nn->topology[i]]();
            }
        }
    }

    // Destructor to clean up dynamically allocated memory
    ~AdamOptimizer()
    {
        for (int i = 0; i < layers - 1; ++i) {
            if (nn->bias_disable == false) {
                delete[] m_bias[i];
                delete[] v_bias[i];
            }
            for (int j = 0; j < nn->topology[i + 1]; ++j) {
                delete[] m_weight[i][j];
                delete[] v_weight[i][j];
            }
            delete[] m_weight[i];
            delete[] v_weight[i];
        }
        if (nn->bias_disable == false) {
            delete[] m_bias;
            delete[] v_bias;
        }
        delete[] m_weight;
        delete[] v_weight;
    }

    // Update function for the entire neural network
    void update()
    {
        calculate_gradients();

        t++; // Increment time step

        float m_hat_divider = 1 - std::pow(beta1, t);
        float v_hat_divider = 1 - std::pow(beta2, t);

        // Loop through all layers (starting from the first hidden layer)
        for (int layer = 1; layer < layers; ++layer) { /////////////////
            // Loop through each neuron in the current layer
            for (int neuron = 0; neuron < neurons; ++neuron) {

                float gradient = me->delta[layer - 1][neuron];

                // Update weights for each neuron in the previous layer
                for (int prev_neuron = 0; prev_neuron < prev_neurons; ++prev_neuron) {
                    // Update weights using Adam optimizer
                    m_weight[layer - 1][neuron][prev_neuron]
                        = beta1 * m_weight[layer - 1][neuron][prev_neuron]
                        + (1 - beta1) * (gradient * nn->activation[prev_layer][prev_neuron]);

                    v_weight[layer - 1][neuron][prev_neuron]
                        = beta2 * v_weight[layer - 1][neuron][prev_neuron]
                        + (1 - beta2) * (gradient * nn->activation[prev_layer][prev_neuron]) * (gradient * nn->activation[prev_layer][prev_neuron]);

                    // Weight correction
                    float m_hat = m_weight[layer - 1][neuron][prev_neuron] / m_hat_divider;
                    float v_hat = v_weight[layer - 1][neuron][prev_neuron] / v_hat_divider;

                    // Update the weight
                    nn->weight[layer - 1][neuron][prev_neuron] -= me->learning_rate * m_hat / (std::sqrt(v_hat) + epsilon);
                }
                if (nn->bias_disable == false) {

                    // Update biases using Adam optimizer
                    m_bias[layer - 1][neuron] = beta1 * m_bias[layer - 1][neuron] + (1 - beta1) * gradient;
                    v_bias[layer - 1][neuron] = beta2 * v_bias[layer - 1][neuron] + (1 - beta2) * gradient * gradient;

                    // Bias correction
                    float m_hat = m_bias[layer - 1][neuron] / m_hat_divider;
                    float v_hat = v_bias[layer - 1][neuron] / v_hat_divider;

                    // Update the bias
                    nn->bias[layer - 1][neuron] -= me->learning_rate_of_bias * m_hat / (std::sqrt(v_hat) + epsilon); // az epsilon egy kis érték, például 1e-8, amely biztosítja, hogy ne találkozzunk nullával osztás hibával
                }
            }
        }
    }
};


NN_Optimizer.h

// https://www.youtube.com/watch?v=NVLXCwc8HzM&t=201s
// https://www.ctrlaltftc.com/the-pid-controller/practical-improvements-to-pid
#pragma once

#include <Arduino.h>

class PID {

private:
    float Kp, Ki, Kd;
    float I_term = 0;
    float last_error = 0;
    uint32_t last_time = 0;
    bool enable = true;

public:
    void set_params(const float Kc, const float Ti, const float Td);
    void set_gains(const float Kp, const float Ki, const float Kd);
    float update(const float error);
    void start() { enable = true; }
    void stop() { enable = false; }
};

void PID::set_params(const float Kc, const float Ti, const float Td)
{
    Kp = Kc;
    (Ti > 0) ? Ki = Kc / Ti : Ki = 0;
    Kd = Kc * Td;
}

void PID::set_gains(const float Kp, const float Ki, const float Kd)
{
    this->Kp = Kp;
    this->Ki = Ki;
    this->Kd = Kd;
}

float PID::update(const float error)
{
    if (!enable) {
        I_term = 0;
        last_time = 0;
        return 0;
    }

    uint32_t time = micros();

    float output = Kp * error;

    if (last_time) {
        float dt = (float)(time - last_time) / 1000000.0; // sec
        if (dt != 0.0) {
            output += Kd * (error - last_error) / dt;
            I_term += Ki * error * dt;
            output += I_term;
        }
    }

    if (output > 1.0) {
        I_term = 0.0;
        output = 1.0;
    } else if (output < -1.0) {
        I_term = 0.0;
        output = -1.0;
    }

    last_time = time;
    last_error = error;

    return output;
}


PID.h

#pragma once

#include <Arduino.h>

//*****************************************************************************
//  Adding Streaming (insertion-style) Output
//  Serial << "My name is " << name << " and I am " << age << " years old.";
//  Serial << "GPS unit #" << gpsno << " reports lat/long of " << lat << "/" << long;
template <class T>
inline Print& operator<<(Print& obj, T arg)
{
    obj.print(arg);
    return obj;
}
//*****************************************************************************

//*****************************************************************************
//
//
//
template <typename T, size_t N>
inline
size_t countOf(T (&)[N]) { return N; }
//*****************************************************************************

#define NumberOf(arg) ((unsigned int)(sizeof(arg) / sizeof(arg[0])))