#ifndef _RC_HANDLER_H
#define _RC_HANDLER_H
#define maxLastRcPackage 2500

#include <SPI.h>
#include <jmutex.h>
#include <jqueue.h>

#include "../structsAndDefines.h"
#include "RF24.h"
#include "nRF24L01.h"

mutex_t radioAccess;

RF24 radio(0, 4);
const uint64_t myPipe = 0xF0F0F0F000 | myId;
byte jRCbuff[32];
unsigned long jLastRCData = 0;
byte jLastStatus = 0;

void initRC() {
    mutex_init(&radioAccess);
    mutex_enter_blocking(&radioAccess);
    if (!radio.begin()) {
        Serial.println("radio hardware is not responding!!");
        while (1) {
        }
    }
    radio.enableDynamicPayloads();
    radio.setPALevel(RF24_PA_HIGH);
    radio.setDataRate(RF24_1MBPS);
    radio.setRetries(5, 15);
    radio.setChannel(myChannel);
    radio.enableDynamicPayloads();
    radio.disableAckPayload();
    radio.setAutoAck(false);
    radio.openReadingPipe(0, myPipe);
    radio.startListening();
    mutex_exit(&radioAccess);
}

bool jSendNRF(const void* buf, uint8_t length) {
    mutex_enter_blocking(&radioAccess);
    radio.stopListening();
    bool succ = radio.write(buf, length);
    radio.startListening();
    mutex_exit(&radioAccess);
    return succ;
}

#include "../rcserial.h"
#include "../system_sensors/compass.h"
#include "../system_sensors/gnss.h"
#include "../system_sensors/mpu.h"
#include "multicast.h"
#include "rc_data.h"

#if debugReceive
const String b16 = "0123456789abcdef";

String SensorDataBuffer = "";
#endif

bool receiveRC() {
    mutex_enter_blocking(&radioAccess);
    if (radio.available()) {
#if debugReceive
        Serial.println("Radio available!");
#endif
        uint8_t payloadSize = radio.getDynamicPayloadSize();
        if (payloadSize >= 1) {
            jLastRCData = millis();
            radio.read(&jRCbuff, payloadSize);
            mutex_exit(&radioAccess);
#if debugReceive
            SensorDataBuffer = "data: ";
            for (size_t i = 0; i < payloadSize; i++) {
                SensorDataBuffer += b16[(jRCbuff[i] >> 4) & 0xf];
                SensorDataBuffer += b16[jRCbuff[i] & 0xf];
            }
            Serial.println(SensorDataBuffer);
#endif
            switch (jRCbuff[0]) {
                case jType_RC: {
                    RCdataradioTask();
                } break;
                case jType_RC_SERIAL: {
                    _rc_serial_add((char*)(jRCbuff + 2), jRCbuff[1]);
                } break;
                case jType_S_MPU: {
                    MPUradioTask();
                } break;
                case jType_S_GNSS: {
                    GNSSradioTask();
                } break;
                case jType_S_COMP: {
                    COMPradioTask();
                } break;
            }
            return true;
        } else {
            mutex_exit(&radioAccess);
            return false;
        }
    } else {
        mutex_exit(&radioAccess);
        return false;
    }
}

void handleRC() {
    while (receiveRC())
        ;
    if (millis() - jLastRCData > maxLastRcPackage) {  // no connection
        if (jLastStatus == 1) {
            if (_j_on_disconnect != nullptr) _j_on_disconnect(true);
            jLastStatus = 3;
            radio.setAutoAck(false);
        }
        RCMulticastTask();
    } else {  // connection
        if (jLastStatus != 1) {
            if (_j_on_connect != nullptr) _j_on_connect();
            radio.openWritingPipe(myPipe);
            radio.setAutoAck(true);
            radio.startListening();
            jLastStatus = 1;
        }
        _handle_serial_send();
    }
    // todo: multicast, send serial
}

#endif