# Template Interface

#include "AquaStorm.h"

// Set the serial connection to the hardware interface connector
AquaStorm::HardwareInterface iface(::Serial2);

// Setup the DSIS Protocol
AquaStorm::Platform platform("aquastorm");
AquaStorm::System eindmaas("eindmaas");
AquaStorm::Module io("io");
AquaStorm::Type::Integer btn("button", "user button"); // Add user button (as integer)
AquaStorm::Type::Integer led("led", "status led"); // Add status led (as integer)
DSIS::Data::Range on_off("0", "1"); // Create an on off range for button

//Add Servo Module
AquaStorm::Module servo("servo");
AquaStorm::Type::Integer pwm("pwm", "");
AquaStorm::Type::Integer speedSetPoint("pwm", "", 1500); //create field and add starting value
DSIS::Data::Range servo_range("500", "2500"); // Set servo range



void setup(){
    // Pin setup
    pinMode(34, INPUT); // User button input
    pinMode(2, OUTPUT); // Status led output

    ledcSetup(0, 50, 16); //Setup PWM functionality on ESP32 (channel, frequency(Hz), resolution(bits))
    ledcAttachPin(19, 0); //Attach PWM output(IO19) to the created channel(0)

    // Network interface connection
    Serial.begin(115200);
    iface.begin(115200, SERIAL_8N1, GPIO_NUM_14, GPIO_NUM_27); //Setup Serial Connection

    // Server connection
    iface.setServer(IPAddress(192, 168, 1, 92), 1883); //Setup MQTT Server
    iface.connect(); // Connect to MQTT Broker

    // Setup LED and Button
    io.addOutput(btn);
    io.addInput(led);
    led.type().setRange(on_off);
    btn.type().setRange(on_off);
    eindmaas.addModule(io);

    // Setup Servo
    servo.addInput(speedSetPoint);
    servo.addOutput(pwm);
    speed.type().setRange(servo_range);
    speedSetPoint.type().setRange(servo_range);
    eindmaas.addModule(servo);


    platform.addSystem(eindmaas);
    platform.setUpdateHandler([](std::string &topic, const char *data){
      delay(5);
      return iface.publish(topic.c_str(), (Mosquitto::payload_t) data, 0, 0, true); }
    );

    //This function executes when servo value has changed
    speedSetPoint.onChange([](int setPoint){
      setPulseWidth(0, setPoint); //set PWM channel to incomming value
      pwm = setPoint; // Update the pwm field
      ::Serial.println("Speed changed: " + String(setPoint)); // Print message to user serial (not the network interface serial!)
    });

    //Log message when packet has been received (debug reasons (not neccesary for workign of the module))
    iface.setOnPacket([](const char *topic, const char *data){
        int res = platform.update(topic, (char*)data, 0);
        Serial.println(res);
    });

    platform.init(); //initialize the created platform

    iface.subscribe("aquastorm/eindmaas/modules/servo/0/inputs/speed", 0); // Subscribe to created topic
    iface.subscribe("aquastorm/eindmaas/modules/io/0/inputs/led", 0); // Subscribe to created topic
}

void loop(){
    btn = digitalRead(34); //Update field with current status
    digitalWrite(2, led.get() == 1); //Update led when status is updated
}

void setPulseWidth(int ledChannel, float pulseWidth){
    uint32_t duty = (pulseWidth / 20000.0 * 65535.0);
    ledcWrite(ledChannel, duty);
}