#include <AP_HAL/AP_HAL.h>
#include "UDPDriver.h"
#include <stdio.h>
#include <stdint.h>
#include <unistd.h>
#include "Scheduler.h"
#include "Storage.h"
#include "replace.h"
#include <qurt_dsp.h>

extern const AP_HAL::HAL& hal;

extern "C" {
    int ArduPilot_main(int argc, const char *argv[]);
}

volatile int _last_dsp_line = __LINE__;
volatile const char *_last_dsp_file = __FILE__;
volatile uint32_t _last_counter;

static void *main_thread(void *)
{
    HAP_PRINTF("Ardupilot main_thread started");
    ArduPilot_main(0, NULL);
    return NULL;
}


uint32_t ardupilot_start()
{
    HAP_PRINTF("Starting Ardupilot");
    pthread_attr_t thread_attr;
    struct sched_param param;
    pthread_t thread_ctx;

    pthread_attr_init(&thread_attr);
    pthread_attr_setstacksize(&thread_attr, 80000);

    param.sched_priority = APM_MAIN_PRIORITY;
    (void)pthread_attr_setschedparam(&thread_attr, &param);

    pthread_create(&thread_ctx, &thread_attr, main_thread, NULL);
    return 0;
}

uint32_t ardupilot_heartbeat()
{
    HAP_PRINTF("tick %u %s:%d", (unsigned)_last_counter, _last_dsp_file, _last_dsp_line);
    return 0;
}

uint32_t ardupilot_set_storage(const uint8_t *buf, int len)
{
    if (len != sizeof(QURT::Storage::buffer)) {
        HAP_PRINTF("Incorrect storage size %u", len);
        return 1;
    }
    QURT::Storage::lock.take(0);
    memcpy(QURT::Storage::buffer, buf, len);
    QURT::Storage::lock.give();
    return 0;
}

uint32_t ardupilot_get_storage(uint8_t *buf, int len)
{
    if (len != sizeof(QURT::Storage::buffer)) {
        HAP_PRINTF("Incorrect storage size %u", len);
        return 1;
    }
    if (!QURT::Storage::dirty) {
        return 1;
    }
    QURT::Storage::lock.take(0);
    memcpy(buf, QURT::Storage::buffer, len);
    QURT::Storage::lock.give();
    return 0;
}


uint32_t ardupilot_socket_check(uint8_t *buf, int len, uint32_t *nbytes)
{
    return QURT::UDPDriver::from(hal.uartA)->socket_check(buf, len, nbytes);
}

uint32_t ardupilot_socket_input(const uint8_t *buf, int len, uint32_t *nbytes)
{
    return QURT::UDPDriver::from(hal.uartA)->socket_input(buf, len, nbytes);
}