/*
 * 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/>.
 *
 * Author: Eugene Shamaev, Siddharth Bharat Purohit
 */
#ifndef AP_UAVCAN_H_
#define AP_UAVCAN_H_

#include <uavcan/uavcan.hpp>
#include "AP_UAVCAN_Server.h"

#include <AP_HAL/CAN.h>
#include <AP_HAL/Semaphores.h>
#include <AP_Param/AP_Param.h>

#include <uavcan/helpers/heap_based_pool_allocator.hpp>

#ifndef UAVCAN_NODE_POOL_SIZE
#define UAVCAN_NODE_POOL_SIZE 8192
#endif

#ifndef UAVCAN_NODE_POOL_BLOCK_SIZE
#define UAVCAN_NODE_POOL_BLOCK_SIZE 64
#endif

#ifndef UAVCAN_SRV_NUMBER
#define UAVCAN_SRV_NUMBER 18
#endif

#define AP_UAVCAN_SW_VERS_MAJOR 1
#define AP_UAVCAN_SW_VERS_MINOR 0

#define AP_UAVCAN_HW_VERS_MAJOR 1
#define AP_UAVCAN_HW_VERS_MINOR 0

#define AP_UAVCAN_MAX_LED_DEVICES 4

// fwd-declare callback classes
class ButtonCb;
class TrafficReportCb;

class ZrCamStateCb;
class ZrCamStatusCb;
class ZrCamVersionCb;
class ZrCamFeedBackCb;
class ZrMessageCb;
class ZrRequestCb;
class ZrCamAmountCb;
typedef struct
{
    uint8_t id;         // bit len 8
    uint8_t gps_state;  // bit len 8
    uint8_t fly_status; // bit len 8
    uint8_t wp_number;
    bool armd; // bit len 1
} zr_uav_state_data_t;

typedef struct
{
    uint32_t week;              // bit len 32
    uint32_t weekms;            // bit len 32
    uint32_t latitude_deg_1e7;  // bit len 32
    uint32_t longitude_deg_1e7; // bit len 32
    uint32_t height_abs_cm;     // bit len 24
    uint16_t index;             // bit len 16
    uint8_t cam_number;
} ppk_pos_data_t;
/*
    Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
    the Callback will invoke registery to register the node as separate backend.
*/
#define UC_REGISTRY_BINDER(ClassName_, DataType_) \
	class ClassName_ : public AP_UAVCAN::RegistryBinder<DataType_> { \
        typedef void (*CN_Registry)(AP_UAVCAN*, uint8_t, const ClassName_&); \
	    public: \
	        ClassName_() : RegistryBinder() {} \
	        ClassName_(AP_UAVCAN* uc,  CN_Registry ffunc) : \
				RegistryBinder(uc, (Registry)ffunc) {} \
	}

class AP_UAVCAN : public AP_HAL::CANProtocol {
public:
    AP_UAVCAN();
    ~AP_UAVCAN();


    typedef struct
    {
        uint8_t software_major;  // bit len 8
        uint8_t software_minor;  // bit len 8
        uint8_t software_patch; // bit len 8
        uint8_t hardware_major;  // bit len 8
        uint8_t hardware_minor;  // bit len 8
        uint8_t hardware_patch; // bit len 8
    } zr_uav_version_data_t;

  

   typedef enum
    {
        ZR_UAVCAN_REQUEST_TIMESTAMP=1,
        ZR_UAVCAN_REQUSET_VERSION,
    }zr_uav_request_e;


    static const struct AP_Param::GroupInfo var_info[];

    // Return uavcan from @driver_index or nullptr if it's not ready or doesn't exist
    static AP_UAVCAN *get_uavcan(uint8_t driver_index);

    void init(uint8_t driver_index, bool enable_filters) override;

    uavcan::Node<0>* get_node() { return _node; }
    uint8_t get_driver_index() { return _driver_index; }


    ///// SRV output /////
    void SRV_push_servos(void);

    ///// LED /////
    bool led_write(uint8_t led_index, uint8_t red, uint8_t green, uint8_t blue);

    // buzzer
    void set_buzzer_tone(float frequency, float duration_s);

    // send RTCMStream packets
    void send_RTCMStream(const uint8_t *data, uint32_t len);

    bool set_zr_rgb_led(uint8_t mode, uint8_t red, uint8_t green, uint8_t blue);
    void set_zr_cam_cmd(uint8_t gimbal_id,  uint8_t mode,uint8_t v1, uint8_t v2,uint8_t v3,uint8_t v4);
    void set_cam_geopoi(uint8_t id,uint8_t mode,int32_t longitude_deg_1e7,int32_t latitude_deg_1e7,int32_t  height_cm,int8_t height_type);
    void set_cam_angular(uint8_t id,uint8_t mode,float pitch,float roll,float yaw);
    void set_zr_timestamp(uint8_t id,uint64_t timestamp);
    void set_zr_uav_state(zr_uav_state_data_t &data);
    void set_zr_uav_version(zr_uav_version_data_t &data);
    void set_zr_ppk_pos(ppk_pos_data_t &data);
    template <typename DataType_>
    class RegistryBinder {
    protected:
        typedef void (*Registry)(AP_UAVCAN* _ap_uavcan, uint8_t _node_id, const RegistryBinder& _cb);
        AP_UAVCAN* _uc;
        Registry _ffunc;

    public:
        RegistryBinder() :
        	_uc(),
            _ffunc(),
            msg() {}

        RegistryBinder(AP_UAVCAN* uc, Registry ffunc) :
            _uc(uc),
            _ffunc(ffunc),
            msg(nullptr) {}

        void operator()(const uavcan::ReceivedDataStructure<DataType_>& _msg) {
            msg = &_msg;
            _ffunc(_uc, _msg.getSrcNodeID().get(), *this);
        }

        const uavcan::ReceivedDataStructure<DataType_> *msg;
    };

private:
    class SystemClock: public uavcan::ISystemClock, uavcan::Noncopyable {
    public:
        SystemClock() = default;

        void adjustUtc(uavcan::UtcDuration adjustment) override {
            utc_adjustment_usec = adjustment.toUSec();
        }

        uavcan::MonotonicTime getMonotonic() const override {
            return uavcan::MonotonicTime::fromUSec(AP_HAL::micros64());
        }

        uavcan::UtcTime getUtc() const override {
            return uavcan::UtcTime::fromUSec(AP_HAL::micros64() + utc_adjustment_usec);
        }

        static SystemClock& instance() {
            static SystemClock inst;
            return inst;
        }

    private:
        int64_t utc_adjustment_usec;
    };

    // This will be needed to implement if UAVCAN is used with multithreading
    // Such cases will be firmware update, etc.
    class RaiiSynchronizer {};

    void loop(void);

    ///// SRV output /////
    void SRV_send_actuator();
    void SRV_send_esc();

    ///// LED /////
    void led_out_send();

    // buzzer
    void buzzer_send();

    // SafetyState
    void safety_state_send();

    // send GNSS injection
    void rtcm_stream_send();

    void  cam_geopoi_send();
    void  cam_aular_send();
    void  zr_cam_cmd_send();
    void  zr_rgb_led_send();
    void  zr_state_send();
    void  zr_uav_timestamp_send();
    void  zr_uav_state_send();
    void  zr_uav_version_send();
    void  zr_uav_pos_send();
    void  zr_uav_euler_send();
    void  zr_uav_ppk_send();
    uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;

    // UAVCAN parameters
    AP_Int8 _uavcan_node;
    AP_Int32 _servo_bm;
    AP_Int32 _esc_bm;
    AP_Int16 _servo_rate_hz;

    uavcan::Node<0> *_node;

    uint8_t _driver_index;
    char _thread_name[9];
    bool _initialized;
    ///// SRV output /////
    struct {
        uint16_t pulse;
        bool esc_pending;
        bool servo_pending;
    } _SRV_conf[UAVCAN_SRV_NUMBER];

    uint8_t _SRV_armed;
    uint32_t _SRV_last_send_us;
    uint32_t _pos_last_send_ms;
    uint32_t _ppk_last_send_ms;
    HAL_Semaphore SRV_sem;
    HAL_Semaphore uav_pos_sem;
    ///// LED /////
    struct led_device {
        uint8_t led_index;
        uint8_t red;
        uint8_t green;
        uint8_t blue;
    };

    struct {
        led_device devices[AP_UAVCAN_MAX_LED_DEVICES];
        uint8_t devices_count;
        uint64_t last_update;
    } _led_conf;

    HAL_Semaphore _led_out_sem;

    // buzzer
    struct {
        HAL_Semaphore sem;
        float frequency;
        float duration;
        uint8_t pending_mask; // mask of interfaces to send to
    } _buzzer;

    // GNSS RTCM injection
    struct {
        HAL_Semaphore sem;
        uint32_t last_send_ms;
        ByteBuffer *buf;
    } _rtcm_stream;

    struct
    {
        HAL_Semaphore sem;      
        uint8_t gimbal_id;
        uint8_t mode;
        uint8_t v1;
        uint8_t v2;
        uint8_t v3;
        uint8_t v4;
        bool need_send;
    } _zr_cam_cmd;

    struct
    {
        HAL_Semaphore sem;
        int32_t longitude_deg_1e7;
        int32_t latitude_deg_1e7;
        int32_t height_cm;
        uint8_t id;
        uint8_t mode;
        int8_t height_type;
        bool need_send;
    } _cam_geopoi;

    struct
    {
        HAL_Semaphore sem;
        uint8_t id;
        uint8_t mode;
        float pitch;
        float roll;
        float yaw;
        bool need_send;
    } _cam_angular;

    struct
    {
        HAL_Semaphore sem;
        uint8_t mode;
        uint8_t red;
        uint8_t green;
        uint8_t blue;
        bool need_send;
    } _zr_rgb_led;

    struct
    {
        HAL_Semaphore sem;
        uint64_t timestamp;
        bool need_send;
    } _zr_uav_timestamp;

    struct
    {
        HAL_Semaphore sem;
        zr_uav_state_data_t data;
        bool need_send;
    } _zr_uav_state;
    struct
    {
        HAL_Semaphore sem;
        zr_uav_version_data_t data;
        bool need_send;
    } _zr_uav_version;

    typedef struct
    {
        // FieldTypes
        uint8_t id;      // bit len 8
        uint8_t command; // bit len 8
        uint8_t param1;  // bit len 8
        uint8_t param2;  // bit len 8
        uint8_t param3;  // bit len 8
        uint8_t param4;  // bit len 8
    } zr_request_t;

    struct
    {
        HAL_Semaphore sem;
        ppk_pos_data_t data;
        bool need_send;
    } _zr_ppk_pos;

    // safety status send state
    uint32_t _last_safety_state_ms;
    uint32_t _last_uav_state_ms;
    uint32_t _last_euler_ms;
    // safety button handling
    static void handle_button(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ButtonCb &cb);
    static void handle_traffic_report(AP_UAVCAN* ap_uavcan, uint8_t node_id, const TrafficReportCb &cb);
    static void handle_zr_camera_old_status(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamStatusCb &cb);
    static void handle_zr_camera_version(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamVersionCb &cb);
    static void handle_zr_camera_feedback(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamFeedBackCb &cb);
    static void handle_zr_message(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrMessageCb &cb);
    static void handle_zr_request(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrRequestCb &cb);
    static void handle_zr_camera_state(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamStateCb &cb);
    static void handle_zr_camera_sum(AP_UAVCAN* ap_uavcan, uint8_t node_id, const ZrCamAmountCb &cb);
};

#endif /* AP_UAVCAN_H_ */