#ifndef jIO_H_
#define jIO_H_

#define esc_freq 250
#define esc_res 12

#define esc_duty (1 << esc_res) / (1000.0 / esc_freq)

#include "structsAndDefines.h"

size_t _io_id_count = 0;
jType _io_types[9] = {jUnused, jUnused, jUnused, jUnused, jUnused,
                      jUnused, jUnused, jUnused, jUnused};

void iniOut(size_t i, jType type) {
    if (type == jServo) {
        _io_types[i] = jServo;
        ledcSetup(i, 50, 16);
        ledcAttachPin(jChannel[i], i);
    } else if (type == jhBridge) {
        _io_types[i] = jhBridge;
        ledcSetup(i, 30000, 8);
        ledcAttachPin(jChannel[i], i);
    } else if (type == jESC) {
        _io_types[i] = jESC;
        ledcSetup(i, esc_freq, esc_res);
        ledcAttachPin(jChannel[i], i);
    } else if (type == jDigital) {
        _io_types[i] = jDigital;
        pinMode(jChannel[i], OUTPUT);
    }
}

bool setoutput(int c, float v) {
    if (sizeof(jChannel) <= c) {
        return false;
    }
    if (_io_types[c] == jServo) {
        // uint32_t duty = (((v/180.0)*2000)/20000.0*65536.0) + 1634;// convert
        // 0-180 degrees to 0-65536
        if (v > 180) v = 180.0;
        if (v < 0) v = 0.0;
        int duty = (((v * 2000.0 / 180.0) + 500) / (20000.0 / pow(2.0, 16.0)));
        Serial.println(duty);
        ledcWrite(c, duty);
        return true;
    } else if (_io_types[c] == jhBridge) {
        if (v > 100) v = 100.0;
        if (v < 0) v = 0.0;
        Serial.println((int)(v * 105.0 / 100.0) + 150);
        ledcWrite(c, (int)(v * 105.0 / 100.0) + 150);
        return true;
    } else if (_io_types[c] == jESC) {
        if (v > 100) v = 100.0;
        if (v < 0) v = 0.0;
        float escout =
            (v * (esc_duty /* highrt - lower = lower*/) / 100.0) + esc_duty;
        ledcWrite(c, escout);
        return true;
    } else if (_io_types[c] == jDigital) {
        if (v > 0) {
            digitalWrite(jChannel[c], HIGH);
        } else {
            digitalWrite(jChannel[c], LOW);
        }
        return true;
    }
    return false;
}

#endif