#pragma once

#include "esphome.h"
#include "Arduino.h"
#include "hciemulator.h"
//#include "cover.h"

#define RS485 Serial2
#define TX_ON 25
//#define configMAX_PRIORITIES 25

#define TAG "hoermann_door"

namespace esphome {
namespace hoermann_door {

HCIEmulator emulator;
TaskHandle_t modBusTask;
void modBusPolling(void *parameter);
class HoermanDoor : public Component, public cover::Cover
{
public:

  HoermanDoor()
  {
    emulator;
  }

  cover::CoverTraits get_traits() override {
    auto traits = cover::CoverTraits();
    traits.set_is_assumed_state(false);
    traits.set_supports_position(true);
    traits.set_supports_tilt(false);
    traits.set_supports_stop(true);
    traits.set_supports_toggle(true);
    return traits;
  }

  //u_int8_t stopAt == 0;
  bool manual = false;
  void control(const cover::CoverCall &call) override {
      ESP_LOGW(TAG, "In func control door");
    // This will be called every time the user requests a state change.
    if (call.get_position().has_value()) {

      float pos = *call.get_position();

      //emulator(&RS485);
      // Write pos (range 0-1) to cover
      // ...
      

      if(pos == 1.0){
        emulator.openDoor();
        manual = false;
      }
      else if(pos == 0.0){
        emulator.closeDoor();
        manual = false;
      }
      else{
        ESP_LOGD("Door controller", "Not yet supported");
      }

    }
    if (call.get_stop()) {
      emulator.stopDoor();
    }
    //if(call.get_close()) {
      //emulator.closeDoor();
    //}
  }

    
    /*
void modBusPolling(void *parameter)
{
  while (true)
  {
    if (lastCall > 0)
    {
      maxPeriod = _max(micros() - lastCall, maxPeriod);
    }
    lastCall = micros();
    emulator.poll();
    vTaskDelay(1);
  }
  vTaskDelete(NULL);
}
    */

    void setup() override
    {
      // setup modbus
      RS485.begin(57600, SERIAL_8E1, 16, 17);
      pinMode(TX_ON, OUTPUT);
      digitalWrite(TX_ON, LOW);
        
      xTaskCreatePinnedToCore(
        modBusPolling,  // Function to implement the task
        "ModBusTask",   // Name of the task
        10000,          // Stack size in words
        NULL,           // Task input parameter
        // 1,           // Priority of the task
        configMAX_PRIORITIES - 1,
        &modBusTask,    // Task handle.
        1               // Core where the task should run
      );


      
      
    }

    


    void open(){
      //emulator.openDoor();
    }
    void close(){
      //emulator.closeDoor();
    }
    void stop(){
      //emulator.stopDoor();
    }

    int count = 0;
    unsigned long lastStatus = 0;
    u_int8_t pos = 255;
    void loop() override
    {
      u_int8_t position = emulator.getState().doorCurrentPosition;
      if(pos != position){
        this->position = (float) position / 200.0;
        this->publish_state();
        pos = position;
      }
    }

};

// Custom binary output, for exposing binary states
class NbsLightOutput: public output::BinaryOutput, public Component {
 public:
  void setup() override {

  }

  void write_state(bool state) override {

  }

  bool lastState = false;
  bool firstState = false;
  void loop() override {
    if(!emulator.getState().valid) false;
    if(!firstState || emulator.getState().lampOn != lastState){
      //ESP_LOGD("Test", "I have no idea");
      lastState = emulator.getState().lampOn;
      this->set_state(lastState);
      firstState = true;
    }
  }

  void turn_on() override {
    if(!lastState) emulator.toggleLamp();
  }
  void turn_off() override {
    if(lastState) emulator.toggleLamp();
  }
};

volatile unsigned long lastCall = 0;
volatile unsigned long maxPeriod = 0;
void modBusPolling(void *parameter)
{
  while (true)
  {
    if (lastCall > 0)
    {
      maxPeriod = _max(micros() - lastCall, maxPeriod);
    }
    lastCall = micros();
    emulator.poll();
    vTaskDelay(1);
  }
  vTaskDelete(NULL);
}

} // namespace hoermann
} // namespace esphome