/*
 * This file is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This file is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Code by Siddharth Bharat Purohit
 */

#include <AP_HAL/AP_HAL.h>
#include <AP_Common/AP_Common.h>

#include "AP_CANManager.h"
#include "AP_CANTester.h"
#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_CANMANAGER_ENABLED && HAL_ENABLE_CANTESTER
#include <AP_SerialManager/AP_SerialManager.h>
#include <stdio.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <AP_UAVCAN/AP_UAVCAN.h>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include <uavcan/equipment/esc/Status.hpp>
#include <uavcan/equipment/esc/RawCommand.hpp>
#include <AP_HAL/utility/sparse-endian.h>
#include "AP_CANTester_KDECAN.h"
extern const AP_HAL::HAL& hal;

const AP_Param::GroupInfo CANTester::var_info[] = {
    // @Param: ID
    // @DisplayName: CAN Test Index
    // @Description: Selects the Index of Test that needs to be run recursively, this value gets reset to 0 at boot.
    // @Range: 0 4
    // @Values: 0:TEST_NONE, 1:TEST_LOOPBACK,2:TEST_BUSOFF_RECOVERY,3:TEST_UAVCAN_DNA,5:TEST_KDE_CAN, 6:TEST_UAVCAN_ESC,  7:TEST_UAVCAN_FD_ESC
    // @User: Advanced
    AP_GROUPINFO("ID", 1, CANTester, _test_id, 0),

    // @Param: LPR8
    // @DisplayName: CANTester LoopRate
    // @Description: Selects the Looprate of Test methods
    // @Units: us
    // @User: Advanced
    AP_GROUPINFO("LPR8", 2, CANTester, _loop_rate, 10000),

    AP_GROUPEND

};

#define debug_can(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "CANTester",  fmt, ##args); } while (0)

bool CANTester::add_interface(AP_HAL::CANIface* can_iface)
{
    if (_num_ifaces >= HAL_NUM_CAN_IFACES) {
        debug_can(AP_CANManager::LOG_ERROR, "Max Number of CanIfaces exceeded");
        return false;
    }

    _can_ifaces[_num_ifaces] = can_iface;

    if (_can_ifaces[_num_ifaces] == nullptr) {
        debug_can(AP_CANManager::LOG_ERROR, "CAN driver not found");
        return false;
    }

    if (!_can_ifaces[_num_ifaces]->is_initialized()) {
        debug_can(AP_CANManager::LOG_ERROR, "Driver not initialized");
        return false;
    }

    _num_ifaces++;
    return true;
}

void CANTester::init(uint8_t driver_index, bool enable_filters)
{
    _driver_index = driver_index;

    // Reset Test mask
    _test_id.set_and_save(0);

    debug_can(AP_CANManager::LOG_DEBUG, "starting init");

    if (_initialized) {
        debug_can(AP_CANManager::LOG_ERROR, "already initialized");
        return;
    }

    if (_can_ifaces[0] == nullptr) {
        debug_can(AP_CANManager::LOG_ERROR, "Interface not found");
        return;
    }

    // kick start tester thread
    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&CANTester::main_thread, void), "can_tester", 4096, AP_HAL::Scheduler::PRIORITY_CAN, 1)) {
        debug_can(AP_CANManager::LOG_ERROR, "couldn't create thread");
        return;
    }

    _initialized = true;

    debug_can(AP_CANManager::LOG_DEBUG, "init done");

    return;
}

// write frame on CAN bus
bool CANTester::write_frame(uint8_t iface, AP_HAL::CANFrame &out_frame, uint64_t timeout)
{
    if (!_can_ifaces[iface]->set_event_handle(&_event_handle)) {
        debug_can(AP_CANManager::LOG_ERROR, "Cannot add event handle");
        return false;
    }
    // wait for space in buffer to send command
    bool read_select = false;
    bool write_select = true;
    out_frame.id += iface; // distinguish between multiple ifaces
    bool ret = _can_ifaces[iface]->select(read_select, write_select, &out_frame, AP_HAL::native_micros64() + timeout);
    if (!ret || !write_select) {
        return false;
    }
    uint64_t deadline = AP_HAL::native_micros64() + 2000000;
    // hal.console->printf("%x TDEAD: %lu\n", out_frame.id, deadline);
    // send frame and return success
    return (_can_ifaces[iface]->send(out_frame, deadline, AP_HAL::CANIface::AbortOnError) == 1);
}

// read frame on CAN bus, returns true on success
bool CANTester::read_frame(uint8_t iface, AP_HAL::CANFrame &recv_frame, uint64_t timeout, AP_HAL::CANIface::CanIOFlags &flags)
{
    if (!_can_ifaces[iface]->set_event_handle(&_event_handle)) {
        debug_can(AP_CANManager::LOG_ERROR, "Cannot add event handle");
        return false;
    }
    // wait for space in buffer to read
    bool read_select = true;
    bool write_select = false;
    bool ret = _can_ifaces[iface]->select(read_select, write_select, nullptr, AP_HAL::native_micros64() + timeout);
    if (!ret || !read_select) {
        // return false if no data is available to read
        return false;
    }
    uint64_t time;

    // read frame and return success
    return (_can_ifaces[iface]->receive(recv_frame, time, flags) == 1);
}

void CANTester::main_thread()
{
    while (true) {
        switch (_test_id) {
        case CANTester::TEST_LOOPBACK:
            if (_can_ifaces[1] != nullptr) {
                gcs().send_text(MAV_SEVERITY_ALERT, "********Running Loopback Test*******");
                if (test_loopback(_loop_rate)) {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********Loopback Test Pass*******");
                } else {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********Loopback Test Fail*******");
                }
            } else {
                gcs().send_text(MAV_SEVERITY_ALERT, "Can't do Loopback Test with single iface");
            }
            break;
        case CANTester::TEST_BUSOFF_RECOVERY:
            if (_can_ifaces[1] != nullptr) {
                gcs().send_text(MAV_SEVERITY_ALERT, "********Running Busoff Recovery Test********");
                if (test_busoff_recovery()) {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********Busoff Recovery Test Pass********");
                } else {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********Busoff Recovery Test Fail********");
                }
            } else {
                gcs().send_text(MAV_SEVERITY_ALERT, "Can't do Busoff Recovery Test with single iface");
            }
            break;
        case CANTester::TEST_UAVCAN_DNA:
            if (_can_ifaces[1] == nullptr) {
                gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN DNA Test********");
                if (test_uavcan_dna()) {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN DNA Test Pass********");
                } else {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN DNA Test Fail********");
                }
            } else {
                gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_DNA_TEST");
            }
            break;
        case CANTester::TEST_KDE_CAN:
            if (_can_ifaces[1] == nullptr) {
                gcs().send_text(MAV_SEVERITY_ALERT, "********Running KDE CAN Test********");
                if (test_kdecan()) {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********KDE CAN Test Pass********");
                } else {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********KDE CAN Test Fail********");
                }
            } else {
                gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for TEST_KDE_CAN");
            }
            break;
        case CANTester::TEST_UAVCAN_ESC:
            if (_can_ifaces[1] == nullptr) {
                gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN ESC Test********");
                if (test_uavcan_esc(false)) {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Pass********");
                } else {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN ESC Test Fail********");
                }
            } else {
                gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_ESC_TEST");
            }
            break;
        case CANTester::TEST_UAVCAN_FD_ESC:
            if (_can_ifaces[1] == nullptr) {
                gcs().send_text(MAV_SEVERITY_ALERT, "********Running UAVCAN FD ESC Test********");
                if (test_uavcan_esc(true)) {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN FD ESC Test Pass********");
                } else {
                    gcs().send_text(MAV_SEVERITY_ALERT, "********UAVCAN FD ESC Test Fail********");
                }
            } else {
                gcs().send_text(MAV_SEVERITY_ALERT, "Only one iface needs to be set for UAVCAN_FD_ESC_TEST");
            }
            break;
        default:
            break;
        }

        for (uint8_t i = 0; i < 2; i++) {
            if (_can_ifaces[i] != nullptr) {
                _can_ifaces[i]->flush_tx();
            }
        }
        hal.scheduler->delay(5000);
        for (uint8_t i = 0; i < 2; i++) {
            if (_can_ifaces[i] != nullptr) {
                _can_ifaces[i]->clear_rx();
            }
        }
    }
}

/*****************************************
 *           Loopback Test               *
 * ***************************************/

#define NUM_LOOPBACK_RUNS 1000UL
#define LOOPBACK_MAGIC 0x34567819UL
#if CONFIG_HAL_BOARD == HAL_BOARD_LINUX ||  CONFIG_HAL_BOARD == HAL_BOARD_SITL
#define NUM_MAX_TX_FRAMES 1
#else
#define NUM_MAX_TX_FRAMES 64 // arbitrary value to max out the buffers
#endif
bool CANTester::test_loopback(uint32_t loop_rate)
{
    AP_HAL::CANFrame frame;
    AP_HAL::CANIface::CanIOFlags flags;
    uint32_t num_loops = NUM_LOOPBACK_RUNS;
    memset(&_loopback_stats[0], 0, sizeof(_loopback_stats[0]));
    memset(&_loopback_stats[1], 0, sizeof(_loopback_stats[1]));

    while (num_loops--) {
        for (uint8_t i = 0; i < 2; i++) {
            // Write as many frames as we can on an iface
            for (uint32_t tx_frames = 0; tx_frames < NUM_MAX_TX_FRAMES; tx_frames++) {
                create_loopback_frame(_loopback_stats[i], frame);
                if (write_frame(i, frame, 0)) {
                    _loopback_stats[i].tx_seq++;
                    _loopback_stats[i].num_tx++;
                } else {
                    break;
                }
            }

            // Also read as much as we can from the second iface
            while (true) {
                reset_frame(frame);
                if (read_frame((i+1)%2, frame, 0, flags)) {
                    if (frame.id != ((13 | AP_HAL::CANFrame::FlagEFF) + i)) {
                        continue;
                    }
                    check_loopback_frame(_loopback_stats[i], frame);
                    _loopback_stats[i].num_rx++;
                } else {
                    break;
                }
            }
        }
        hal.scheduler->delay_microseconds(loop_rate);
    }
    _can_ifaces[0]->flush_tx();
    hal.scheduler->delay_microseconds(1000);
    _can_ifaces[1]->flush_tx();
    hal.scheduler->delay_microseconds(1000);
    // flush the rx data still buffered in the interface
    for (uint8_t i = 0; i < 2; i++) {
        while (true) {
            reset_frame(frame);
            if (read_frame((i+1)%2, frame, 0, flags)) {
                if (frame.id != ((13 | AP_HAL::CANFrame::FlagEFF) + i)) {
                    continue;
                }
                check_loopback_frame(_loopback_stats[i], frame);
                _loopback_stats[i].num_flushed_rx++;
            } else {
                break;
            }
        }
    }

    for (uint8_t i = 0; i < _num_ifaces; i++) {
        DEV_PRINTF("Loopback Test Results %d->%d:\n", i, (i+1)%2);
        DEV_PRINTF("num_tx: %lu, failed_tx: %lu\n",
                            (long unsigned int)_loopback_stats[i].num_tx, (long unsigned int)_loopback_stats[i].failed_tx);
        DEV_PRINTF("num_rx: %lu, flushed_rx: %lu, bad_rx_data: %lu, bad_rx_seq: %lu\n",
                            (long unsigned int)_loopback_stats[i].num_rx, (long unsigned int)_loopback_stats[i].num_flushed_rx,
                            (long unsigned int)_loopback_stats[i].bad_rx_data, (long unsigned int)_loopback_stats[i].bad_rx_seq);
        if (_loopback_stats[i].num_rx < 0.9f * _loopback_stats[i].num_tx ||
            _loopback_stats[i].failed_tx || _loopback_stats[i].bad_rx_seq ||
            _loopback_stats[i].bad_rx_data) {
            return false;
        }
    }
    return true;
}

void CANTester::reset_frame(AP_HAL::CANFrame& frame)
{
    frame.id = 0;
    memset(frame.data, 0, sizeof(frame.data));
    frame.dlc = 0;
}

void CANTester::create_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL::CANFrame& frame)
{
    frame.id = 13 | AP_HAL::CANFrame::FlagEFF;
    frame.dlc = 8;
    frame.setCanFD(stats.tx_seq%2); //every other frame is canfd if supported
    memcpy(frame.data, &stats.tx_seq, sizeof(stats.tx_seq));
    uint32_t loopback_magic = LOOPBACK_MAGIC;
    memcpy(&frame.data[4], &loopback_magic, sizeof(loopback_magic));
}


void CANTester::check_loopback_frame(CANTester::loopback_stats_s &stats, AP_HAL::CANFrame& frame)
{
    if (frame.dlc != 8) {
        stats.bad_rx_data++;
    }

    uint32_t loopback_magic = LOOPBACK_MAGIC;
    if (memcmp(&frame.data[4], &loopback_magic, sizeof(loopback_magic)) != 0) {
        stats.bad_rx_data++;
        return;
    }

    uint16_t curr_seq;

    memcpy(&curr_seq, frame.data, sizeof(curr_seq));

    if (stats.next_valid_seq != curr_seq) {
        stats.bad_rx_seq++;
    }
    stats.next_valid_seq  = curr_seq + 1;
}

/*****************************************
 *         Busoff Recovery Test          *
 * ***************************************/
bool CANTester::test_busoff_recovery()
{
    uint32_t num_busoff_runs = 100000;
    uint64_t timestamp;
    AP_HAL::CANIface::CanIOFlags flags;
    AP_HAL::CANFrame bo_frame;
    bo_frame.id = (10 | AP_HAL::CANFrame::FlagEFF);
    memset(bo_frame.data, 0xA, sizeof(bo_frame.data));
    bo_frame.dlc =8;//AP_HAL::CANFrame::MaxDataLen;
    bool bus_off_detected = false;
    // Bus Fault can be introduced by shorting CANH and CANL
    gcs().send_text(MAV_SEVERITY_ERROR, "Introduce Bus Off Fault on the bus.");
    while (num_busoff_runs--) {
        if (bus_off_detected) {
            break;
        }
        //Spam the bus with same frame
        _can_ifaces[0]->send(bo_frame, AP_HAL::native_micros64()+1000, 0);
        _can_ifaces[1]->receive(bo_frame, timestamp, flags);
        _can_ifaces[1]->send(bo_frame, AP_HAL::native_micros64()+1000, 0);
        _can_ifaces[0]->receive(bo_frame, timestamp, flags);
        bus_off_detected = _can_ifaces[0]->is_busoff() || _can_ifaces[1]->is_busoff();
        hal.scheduler->delay_microseconds(50);
    }
    if (!bus_off_detected) {
        gcs().send_text(MAV_SEVERITY_ERROR, "BusOff not detected on the bus");
        return false;
    }
    gcs().send_text(MAV_SEVERITY_ERROR, "BusOff detected remove Fault.");
    hal.scheduler->delay(4000);
    gcs().send_text(MAV_SEVERITY_ERROR, "Running Loopback test.");
    //Send Dummy Frames to clear the error
    while (!write_frame(0, bo_frame,100)) {}
    bo_frame.id -= 1;
    while (!write_frame(1, bo_frame,100)) {}
    //Clear the CAN bus Rx Buffer
    hal.scheduler->delay(1000);
    _can_ifaces[0]->clear_rx();
    _can_ifaces[1]->clear_rx();

    return test_loopback(_loop_rate);
}

/*****************************************
 *             UAVCAN DNA Test           *
 * ***************************************/

bool CANTester::test_uavcan_dna()
{
    uavcan::CanIfaceMgr _uavcan_iface_mgr {};

    if (!_uavcan_iface_mgr.add_interface(_can_ifaces[0])) {
        gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to add iface");
        return false;
    }

    auto *node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
    if (!node) {
        return false;
    }
    node->setName("org.ardupilot.dnatest");

    uavcan::protocol::HardwareVersion hw_version;
    const uint8_t uid_buf_len = hw_version.unique_id.capacity();
    uint8_t uid_len = uid_buf_len;
    uint8_t unique_id[uid_buf_len];

    if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
        unique_id[uid_len - 1] -= 5;
        uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
    }

    node->setHardwareVersion(hw_version); // Copying the value to the node's internals

    /*
     * Starting the node normally, in passive mode (i.e. without node ID assigned).
     */
    const int node_start_res = node->start();
    if (node_start_res < 0) {
        gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to start the node");
        delete node;
        return false;
    }

    /*
     * Initializing the dynamic node ID allocation client.
     * By default, the client will use TransferPriority::OneHigherThanLowest for communications with the allocator;
     * this can be overriden through the third argument to the start() method.
     */
    auto *client = new uavcan::DynamicNodeIDClient(*node);
    if (!client) {
        delete node;
        return false;
    }
    int expected_node_id = 100;
    int client_start_res = client->start(node->getHardwareVersion().unique_id,    // USING THE SAME UNIQUE ID AS ABOVE
                                         expected_node_id);
    if (client_start_res < 0) {
        gcs().send_text(MAV_SEVERITY_ALERT,"Failed to start the dynamic node");
    }

    /*
     * Waiting for the client to obtain for us a node ID.
     * This may take a few seconds.
     */
    gcs().send_text(MAV_SEVERITY_ALERT, "Allocation is in progress");
    uint32_t num_runs = 100;
    while (!client->isAllocationComplete() && num_runs--) {
        const int res = node->spin(uavcan::MonotonicDuration::fromMSec(200));    // Spin duration doesn't matter
        if (res < 0) {
            gcs().send_text(MAV_SEVERITY_ALERT, "Transient failure");
        }
    }
    gcs().send_text(MAV_SEVERITY_ALERT, "Dynamic NodeID %d allocated node ID %d",
                    int(client->getAllocatedNodeID().get()),
                    int(client->getAllocatorNodeID().get()));
    if (client->getAllocatedNodeID().get() != expected_node_id) {
        gcs().send_text(MAV_SEVERITY_ALERT, "Unexpected Node Id, expected %d", expected_node_id);
        delete client;
        delete node;
        return false;
    }
    delete client;
    delete node;
    return true;
}

/*****************************************
 *              KDE CAN Test             *
 *****************************************/
bool CANTester::test_kdecan()
{
    AP_CANTester_KDECAN* kdecan_test = new AP_CANTester_KDECAN;
    if (kdecan_test == nullptr) {
        gcs().send_text(MAV_SEVERITY_ERROR, "Failed to allocate KDECAN Tester");
        return false;
    }
    kdecan_test->init(_can_ifaces[0]);

    while (true) {
        kdecan_test->loop();
        static uint32_t last_print_ms;
        static uint32_t last_enumsend_ms;
        static uint8_t enum_count = 0;
        uint32_t now = AP_HAL::millis();
        if (now - last_print_ms >= 1000) {
            last_print_ms = now;
            kdecan_test->print_stats();
        }
        if (!_kdecan_enumeration) {
            enum_count = 0;
        }
        if (now - last_enumsend_ms >= 2000 && enum_count < 4 && _kdecan_enumeration) {
            last_enumsend_ms = now;
            if (kdecan_test->send_enumeration(enum_count)) {
                enum_count++;
            }
        }
    }
    return true;
}

bool CANTester::run_kdecan_enumeration(bool start_stop)
{
    _kdecan_enumeration = start_stop;
    return true;
}

/***********************************************
 *                UAVCAN ESC                   *
 * *********************************************/

#define NUM_ESCS 4
static uavcan::Publisher<uavcan::equipment::esc::Status>* esc_status_publisher;
static uavcan::Subscriber<uavcan::equipment::esc::RawCommand> *esc_command_listener;
static uint16_t uavcan_esc_command_rate = 0;

void handle_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand>& msg);
void handle_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand>& msg)
{
    static uint16_t num_received = 0;
    static uint32_t last_millis;
    if (num_received == 0) {
        last_millis = AP_HAL::millis();
    }
    num_received++;
    // update rate every 50 packets
    if (num_received == 50) {
        uavcan_esc_command_rate = 50000/(AP_HAL::millis() - last_millis);
        num_received = 0;
    }
}

bool CANTester::test_uavcan_esc(bool enable_canfd)
{
    bool ret = true;
    uavcan::CanIfaceMgr _uavcan_iface_mgr {};

    if (!_uavcan_iface_mgr.add_interface(_can_ifaces[0])) {
        gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to add iface");
        return false;
    }

    uavcan::Node<0> *node = nullptr;
    {
        node = new uavcan::Node<0>(_uavcan_iface_mgr, uavcan::SystemClock::instance(), _node_allocator);
        if (node == nullptr) {
            gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to allocate ESC Node");
            ret = false;
            goto exit;
        } else {
            node->setName("org.ardupilot.esctest");
        }
    }

    {
        uavcan::protocol::HardwareVersion hw_version;
        const uint8_t uid_buf_len = hw_version.unique_id.capacity();
        uint8_t uid_len = uid_buf_len;
        uint8_t unique_id[uid_buf_len];
        if (hal.util->get_system_id_unformatted(unique_id, uid_len)) {
            // Generate random uid
            unique_id[uid_len - 1] += 5;
            uavcan::copy(unique_id, unique_id + uid_len, hw_version.unique_id.begin());
        }

        node->setHardwareVersion(hw_version); // Copying the value to the node's internals
    }
    /*
    * Starting the node normally, in passive mode (i.e. without node ID assigned).
    */
    {
        const int node_start_res = node->start();
        if (node_start_res < 0) {
            gcs().send_text(MAV_SEVERITY_CRITICAL, "Failed to start the node");
            ret = false;
            goto exit;
        }
    }

    {
        /*
        * Initializing the dynamic node ID allocation client.
        * By default, the client will use TransferPriority::OneHigherThanLowest for communications with the allocator;
        * this can be overriden through the third argument to the start() method.
        */
        uavcan::DynamicNodeIDClient client(*node);
        int client_start_res = client.start(node->getHardwareVersion().unique_id,    // USING THE SAME UNIQUE ID AS ABOVE
                                            uavcan::NodeID(0));
        if (client_start_res < 0) {
            gcs().send_text(MAV_SEVERITY_ALERT,"Failed to start the dynamic node");
            ret = false;
            goto exit;
        }

        /*
        * Waiting for the client to obtain for us a node ID.
        * This may take a few seconds.
        */
        gcs().send_text(MAV_SEVERITY_ALERT, "Allocation is in progress");
        while (!client.isAllocationComplete()) {
            const int res = node->spin(uavcan::MonotonicDuration::fromMSec(200));    // Spin duration doesn't matter
            if (res < 0) {
                gcs().send_text(MAV_SEVERITY_ALERT, "Transient failure");
            }
        }
        gcs().send_text(MAV_SEVERITY_ALERT, "Dynamic NodeID %d allocated node ID %d",
                        int(client.getAllocatedNodeID().get()),
                        int(client.getAllocatorNodeID().get()));
        if (client.getAllocatedNodeID().get() == 255) {
            gcs().send_text(MAV_SEVERITY_ALERT, "Node Allocation Failed");
            ret = false;
            goto exit;
        }
        esc_command_listener = new uavcan::Subscriber<uavcan::equipment::esc::RawCommand>(*node);
        if (esc_command_listener) {
            esc_command_listener->start(handle_raw_command);
        } else {
            ret = false;
            goto exit;
        }

        esc_status_publisher = new uavcan::Publisher<uavcan::equipment::esc::Status>(*node);
        if (esc_status_publisher == nullptr) {
            ret = false;
            goto exit;
        }
        esc_status_publisher->setTxTimeout(uavcan::MonotonicDuration::fromMSec(2));
        esc_status_publisher->setPriority(uavcan::TransferPriority::OneLowerThanHighest);

        if (enable_canfd) {
            node->enableCanFd();
        } else {
            node->disableCanFd();
        }
        node->setNodeID(client.getAllocatedNodeID());
        node->setModeOperational();
    }

    // Allocations done lets begin
    if (ret) {
        while (true) {
            node->spin(uavcan::MonotonicDuration::fromMSec(1000));
            gcs().send_text(MAV_SEVERITY_ALERT, "UC ESC Command Rate: %d", uavcan_esc_command_rate);
            uavcan_esc_command_rate = 0;
            // send fake ESC stats as well
            for (uint8_t i = 0; i < NUM_ESCS; i++) {
                uavcan::equipment::esc::Status status_msg;
                status_msg.esc_index = i;
                status_msg.error_count = 0;
                status_msg.voltage = 30 + 2*((float)get_random16()/INT16_MAX);
                status_msg.current = 10 + 10*((float)get_random16()/INT16_MAX);
                status_msg.temperature = C_TO_KELVIN(124 + i);
                status_msg.rpm = 1200 + 300*((float)get_random16()/INT16_MAX);
                status_msg.power_rating_pct = 70 + 20*((float)get_random16()/INT16_MAX);
                esc_status_publisher->broadcast(status_msg);
            }
        }
    }

exit:
    // Clean up!
    delete node;

    if (esc_command_listener != nullptr) {
        delete esc_command_listener;
        esc_command_listener = nullptr;
    }
    if (esc_status_publisher != nullptr) {
        delete esc_status_publisher;
        esc_status_publisher = nullptr;
    }
    return ret;
}


CANTester *CANTester::get_cantester(uint8_t driver_index)
{
    if (driver_index >= AP::can().get_num_drivers() ||
        AP::can().get_driver_type(driver_index) != AP_CANManager::Driver_Type_CANTester) {
        return nullptr;
    }
    return static_cast<CANTester*>(AP::can().get_driver(driver_index));
}
#endif //#if HAL_MAX_CAN_PROTOCOL_DRIVERS > 1 && !HAL_MINIMIZE_FEATURES && HAL_MAX_CAN_PROTOCOL_DRIVERS