#ifndef __RTL__
 
#include <stdio.h>
#include <stdlib.h>
#include <errno.h>
#include <stdarg.h>
#include <string.h>

#else /*__RTL__*/

#include <rtl.h>
#include <string.h>
#include <signal.h>
#include <posix/unistd.h>

#endif /*__RTL__*/

#include "ul_utmalloc.h"
#include "ul_gavl.h"
#include "ul_gavlcust.h"
#include "ul_evcbase.h"
#include "sui_event.h"
#include "sui_dinfo.h"
#include "sui_dievc.h"
#include "can_vca.h"
#include "vca_pdo.h"
#include "vcasdo_msg.h"

//====================== logging support =======================
static int log_is_cont = 0;

// -------------- log levels ------------------
#define LOG_FATAL   0
#define LOG_ERR     1
#define LOG_MSG     2
#define LOG_INF     3
#define LOG_DEB     4
#define LOG_TRASH   5
#define LOG_MY      LOG_DEB

static void myvlog(int level, const char *format, va_list ap)
{
    if(log_is_cont) level |= VCA_LOGL_CONT;
    vca_vlog("vcapdo", level, format, ap);
}

static void mylog(int level, const char *format, ...)
{
    va_list ap; 
    va_start(ap, format); 
    myvlog(level, format, ap);
    va_end(ap);
}
/*
static void fatal_error(const char *format, ...)
{
    va_list ap; 
    va_start(ap, format); 
    myvlog(LOG_FATAL, format, ap);
    va_end(ap);
    exit(-1);
}
*/
//=================================================================
//                  PDO list
//=================================================================
GAVL_CUST_NODE_INT_IMP(vcapdolst, vcapdolst_root_t, vcapdolst_object_t, vcapdolst_key_t,
	my_root, my_node, cob_id, vcapdolst_cmp_fnc)
//=================================================================

//-----------------------------------------------------------------
static void _pdo_destroy(vcapdolst_object_t *o)
{
    //free mapped objects
    if(o->mapped_objects) free(o->mapped_objects);
    o->mapped_cnt = 0;
    o->mapped_objects = NULL;
}
//-----------------------------------------------------------------
static vcapdolst_object_t* _pdo_init(vcapdolst_object_t *o)
{
    if(!o) o = (vcapdolst_object_t*)malloc(sizeof(vcapdolst_object_t));
    if(o) {
        o->mapped_cnt = 0;
        o->mapped_objects = NULL;
        o->flags = 0;
        o->sync_counter = 0;
    }
    return o;
}
//-----------------------------------------------------------------
/*
 * _pdo_load_definition_from_OD - loads PDO object acording to its definition in OD
 * 
 * Return:  negative value in case of object creation error or if definition in OD is not valid
 */
static int _pdo_load_definition_from_OD(vcapdolst_object_t *o, vcaod_root_t *odroot, int comm_index, int map_index, int node)
{
    uint32_t err;
    uint32_t u;
    int i, start = 0;
    int res, ret = -1;
    int cnt;
    do {
        if(_vcaod_find_object(odroot, comm_index, 0, NULL) == NULL) return -1;
        if(comm_index < 0x1600) o->flags |= (1<<vcapdoFlagRPDO);
        mylog(LOG_INF, "init %cPDO from definition on %04x\n", (o->flags & (1<<vcapdoFlagRPDO))? 'R': 'T', comm_index);
        res = vcaod_get_value(odroot, comm_index, 0, &u, sizeof(u), &err); if(res < 0) break;
        cnt = u;
        //mylog(LOG_DEB, "\tcommunication profile entries cnt: %i\n", cnt);
        // get cob id    
        /*    
        bit number      value       meaning 
        31 (MSB)        0           PDO exists / is valid 
                        1           PDO does not exist / is not valid 
        30              0           RTR allowed on this PDO 
                        1           no RTR allowed on this PDO 
        29              0           11-bit ID (CAN 2.0A) 
                        1           29-bit ID (CAN 2.0B) 
        28-11           0           if bit 29=0 
                        X           if bit 29=1: bits 28-11 of 29-bit-COB-ID 
        10-0 (LSB)      X           bits 10-0 of COB-ID
        */
        res = vcaod_get_value(odroot, comm_index, 1, &u, sizeof(u), &err); if(res < 0) break;
        mylog(LOG_TRASH, "\tsubindex 1: %04x\n", u);
        if(u==0 && node==0) {
            mylog(LOG_DEB, "\tCOB-ID can not be 0\n");
            return -1;
        }
        else if(u==0 && node>0) {
            // assign default values
            // RPDOs
            // Index 1400h: 200h + Node-ID
            // Index 1401h: 300h + Node-ID
            // Index 1402h: 400h + Node-ID
            // Index 1403h: 500h + Node-ID
            // Index 1404h - 15FFh: disabled
            // TPDOs
            // Index 1800h: 180h + Node-ID
            // Index 1801h: 280h + Node-ID
            // Index 1802h: 380h + Node-ID
            // Index 1803h: 480h + Node-ID
            // Index 1804h - 18FFh: disabled
            int i;
            if(o->flags & (1<<vcapdoFlagRPDO)) {
                i = comm_index - 0x1400;
                if(i>=0 && i<=3) u = 0x200 + 0x100*i + node;
            }
            else {
                i = comm_index - 0x1800;
                if(i>=0 && i<=3) u = 0x180 + 0x100*i + node;
            }
        }
        if(u==0) {
            mylog(LOG_DEB, "\tCOB-ID can not be 0\n");
            return -1;
        }

        if(u & (1<<31)) o->flags |= (1<<vcapdoFlagNotValid);
        if(u & (1<<30)) o->flags |= (1<<vcapdoFlagRTRAllowed);
        if(u & (1<<29)) {
            o->flags |= (1<<vcapdoFlag29BitId);
            u &= 0x1FFFFFFF;
        }
        else {
            u &= 0x7FF;
        }
        if(o->flags & (1<<vcapdoFlagNotValid)) {
            mylog(LOG_DEB, "\tPDO is not valid\n");
            return -1;
        }
        mylog(LOG_DEB, "\tCOB-ID: %04x\n", u);
        o->cob_id = u;
        /* transmission type
         transmission type  cyclic  acyclic synchronous asynchronous    RTR only
         0                             X         X 
         1-240                X                  X (every n-th SYNC)
         241-251 - reserved
         252                                     X                          X 
         253                                                   X            X 
         254                                                   X 
         255                                                   X
         */
        res = vcaod_get_value(odroot, comm_index, 2, &u, sizeof(u), &err); if(res < 0) break;
        o->transmition_type = (unsigned char)u;
        if(u == 0) {
            o->flags |= vcapdoFlagTransSynchronous;
        } else if(u <= 240) {
            o->flags |= vcapdoFlagTransSynchronous;
            o->flags |= vcapdoFlagTransCyclic;
            o->sync_every = (unsigned char)u;
        }
        else if(u < 252) {
            o->flags |= vcapdoFlagTransReserved;
        }
        else if(u == 252) {
            o->flags |= vcapdoFlagTransSynchronous;
            o->flags |= vcapdoFlagTransRTROnly;
        }
        else if(u == 253) {
            o->flags |= vcapdoFlagTransRTROnly;
        }
        mylog(LOG_DEB, "\tTransmition type: %i\n", o->transmition_type);
        
        //inhibit time (optional)
        if(cnt >= 3) {
            res = vcaod_get_value(odroot, comm_index, 3, &u, sizeof(u), &err); 
            if(res < 0) o->inhibit_time = 0;
            else        o->inhibit_time = (uint16_t)u;
            mylog(LOG_DEB, "\tInhibit time: %i\n", o->inhibit_time);
        }
        //event timer (optional)
        if(cnt >= 5) {
            res = vcaod_get_value(odroot, comm_index, 5, &u, sizeof(u), &err);
            if(res < 0) o->event_timer = 0;
            else        o->event_timer = (uint16_t)u;
            mylog(LOG_DEB, "\tEvent timer: %i\n", o->event_timer);
        }
        
        // PDO Mapping
        // number of mapped objects
        res = vcaod_get_value(odroot, map_index, 0, &u, sizeof(u), &err); if(res < 0) break; 
        if(u > 64) u = 64;
        o->mapped_cnt = u;
        o->mapped_objects = (vcapdo_mapping_t*)malloc(o->mapped_cnt * sizeof(vcapdo_mapping_t));
        mylog(LOG_DEB, "\tPDO Mapping:\n");
        mylog(LOG_DEB, "\t\tMapped objects: %i\n", o->mapped_cnt);
        for(i=0; i<o->mapped_cnt; i++) {
            vcapdo_mapping_t *m = o->mapped_objects + i;
            int ix, six, l;
            m->dinfo = NULL;
            res = vcaod_get_value(odroot, map_index, i+1, &u, sizeof(u), &err); if(res < 0) break;
            ix = u >> 16;
            six = (u >> 8) & 0xFF;
            l = u & 0xFF;
            m->object = _vcaod_find_object(odroot, ix, six, &err); if(res < 0) break;
            if(start + l > 64) {
                err = 0x08000023;
                mylog(LOG_ERR, "PDO length exceeds 64 bits\n");
                break;
            }
            m->start = start;
            m->len =  l;
            start += l;
            mylog(LOG_DEB, "\t\t[%2i] object %04x:%02x -> bits[%02i-%02i]\n", i, ix, six, m->start, m->start+m->len-1);
        }
        if(i < o->mapped_cnt) break;
        ret = 0;
    } while(0);
    
    if(ret < 0) {
        o->flags |= (1<<vcapdoFlagNotValid);
        mylog(LOG_ERR, "creating PDO on %04x ... ERROR (%x)\n\t'%s'\n", comm_index, err, vcasdo_abort_msg(err));
    }    
    return ret;
}
//-----------------------------------------------------------------
//=================================================================
//                  vcaPDOProcessor
//=================================================================
void vcaPDOProcessor_init(vcaPDOProcessor_t *proc)
{
    if(!proc) {
        mylog(LOG_ERR, "vcaPDOProcessor_init(): NULL pointer initialized\n");
        return;
    }
    memset(proc, 0, sizeof(vcaPDOProcessor_t));
    vcapdolst_init_root_field(&(proc->pdolst_root));
}
//-----------------------------------------------------------------
void vcaPDOProcessor_destroy(vcaPDOProcessor_t *proc)
{
    if(!proc) {
        mylog(LOG_ERR, "vcaPDOProcessor_init(): NULL pointer destroyed\n");
        return;
    }
    else {
        int i=0;
        vcapdolst_object_t *o;
        vcapdolst_root_t *pdoroot = &(proc->pdolst_root);
        //mylog(LOG_DEB, "destroying PDO structures\n");
        //for(o=vcapdolst_first(pdoroot); o; o=vcapdolst_next(pdoroot, o)) mylog(LOG_DEB, "destroying PDO structure\n");
        while((o = vcapdolst_cut_first(pdoroot)) != NULL) {
            _pdo_destroy(o);
            free(o);
            i++;
        }
        if(i > 0) mylog(LOG_DEB, "vcaPDOProcessor_destroy(): %i PDO structures deleted\n", i);
    }
}
//-----------------------------------------------------------------
// for buffers of length 64bits
// dest_start_bit: 0-63
// src_start_bit: 0-63
static void _bitcpy64(void *dest, int dest_start_bit, void *src, int src_start_bit, int len)
{
    uint64_t ll, mask = 0;
    mask = ~mask;
    mask = ~(mask << len);
    ll = *(uint64_t*)src;
    ll >>= src_start_bit;
    ll &= mask;
    ll <<= dest_start_bit;
    mask <<= dest_start_bit;
    *(uint64_t*)dest &= ~mask;
    *(uint64_t*)dest |= ll;
}
//.................................................................
static void _pdo_hub_event(vcapdolst_object_t *pdo, sui_event_t *event)
{
    //ul_dbuff_t db;
    canmsg_t msg;
    int map_ix;
    sui_dinfo_t *di = (sui_dinfo_t*)(event->u_u.message.ptr);
    vcaPDOProcessor_t *proc=NULL;
    /*
    #if UL_DBUFF_SLEN == CAN_MSG_LENGTH
    ul_dbuff_init(&db, UL_DBUFF_IS_STATIC);
    #else
    ul_dbuff_init(&db, 0);
    #endif
    */
    mylog(LOG_MY, "ENTERING _pdo_hub_event(pdo: %p, event: %p): dinfo: %p proc: %p\n", pdo, event, di, proc);
    if(di && pdo) {
        proc = pdo->pdo_processor;
        if(proc) {
            // find mapping using this hub & dinfo
            for(map_ix=0; map_ix<pdo->mapped_cnt; map_ix++) {
                if(pdo->mapped_objects[map_ix].dinfo == di) {
                    /* dinfo mapped there, more mapping of the same dinfo to one PDO can exist
                     * FIXME: it would be better to schedule PDO for update only 
                     * and left actual sui_rd_xxx to the timer or outer loop
                     */
                    vcapdo_mapping_t *m = pdo->mapped_objects + map_ix;
                    if(m->len <= sizeof(long)*8) {
                        long val;
                        int ret = sui_rd_long(di, 0, &val);
                        if(ret == SUDI_DATA_OK) {
                            mylog(LOG_DEB, "_pdo_hub_event(): for map_x %d read %x\n", map_ix, val);
                            _bitcpy64(pdo->pdo_buff, m->start, &val, 0, m->len);
                            memset(&msg, 0, sizeof(canmsg_t));
                            msg.length = CAN_MSG_LENGTH;
                            memcpy(msg.data, pdo->pdo_buff, msg.length);
                            msg.id = pdo->cob_id;
                            if(proc->send_to_can_fnc) {
                                mylog(LOG_DEB, "_pdo_hub_event(): sending CAN msg id: %04x\n", msg.id);
                                proc->send_to_can_fnc(&msg);
                            }
                        }
                        else {
                            mylog(LOG_ERR, "_pdo_hub_event(): ERROR - %i reading dinfo\n", ret);
                        }
                    }
                    else {
                        mylog(LOG_ERR, "_pdo_hub_event(): handling longer objects than long is not implemented yet\n");
                    }
                }
            }
        }
    }
    mylog(LOG_MY, "_pdo_hub_event(): EXIT proc: %p\n", proc);
}
//.................................................................

int vcaPDOProcessor_createPDOList(vcaPDOProcessor_t *proc)
{
    vcapdolst_object_t *o = NULL;
    int i,j;
    int cnt=0;
    
    mylog(LOG_INF, "Creating PDO structures\n");
    if(!proc) {
        mylog(LOG_ERR, "vcaPDOProcessor_createPDOLIst(): PDO Processor is NULL\n");
        return -1;
    }
    if(!proc->od_root) {
        mylog(LOG_ERR, "vcaPDOProcessor_createPDOLIst(): OD is NULL\n");
        return -1;
    }
    
    vcaPDOProcessor_destroy(proc);
    
    // RPDOs & TPDOs
    // scan wellknowns indexes to find PDO description
    // RPDOs: 1400 - 512 * communication parameter
    // RPDOs: 1600 - 512 * mapping parameter
    // TPDOs: 1800 - 512 * communication parameter
    // TPDOs: 1A00 - 512 * mapping parameter
    for(i=0; i<2; i++) { 
        for(j=0; j<512; j++) {
            int comm_ix = 0x1400+i*0x400+j;
            int map_ix = comm_ix + 0x200;
            int ret;
            vcapdolst_object_t *o = _pdo_init(NULL);
            if(!o) break;
            ret = _pdo_load_definition_from_OD(o, proc->od_root, comm_ix, map_ix, proc->node_id);
            if(ret < 0) {
                _pdo_destroy(o);
                free(o);
                continue;
            }
            ret = vcapdolst_insert(&(proc->pdolst_root), o);
            o->pdo_processor = proc;
            evc_rx_hub_init(&o->rx_hub, (evc_rx_fnc_t*)_pdo_hub_event, o);
            mylog(LOG_MY, "rx_hub(%p) had setted context proc(%p)\n", &o->rx_hub, proc);
            if(ret <= 0) mylog(LOG_DEB, "ERROR inserting PDO[%i] to PDOlist\n", cnt);
            else cnt++;
        }
    }
    // delete last unsuccessfully created PDO structure, if any
    if(o) {
        _pdo_destroy(o);
        free(o);
    }
    mylog(LOG_DEB, "vcaPDOProcessor_createPDOLIst(): %i PDO structures created\n", cnt);
    return 0;
}
//-----------------------------------------------------------------
void _vcaPDOProcessor_disconnectDinfoLinks(vcaPDOProcessor_t *proc)
{
    vcapdolst_object_t *o;
    vcapdolst_root_t *r;
    if(!proc) return;
    
    r = &(proc->pdolst_root);
    for(o = vcapdolst_first(r); o; o = vcapdolst_next(r, o)) {
        vcapdo_mapping_t *m = o->mapped_objects;
        int i;
        for(i=0; i<o->mapped_cnt; i++) {
            sui_dinfo_t *d = m[i].dinfo;
            if(d) sui_dinfo_dec_refcnt(d);
            d = NULL;
        }
    }
}
//-----------------------------------------------------------------
void vcaPDOProcessor_makeDinfoLinks(vcaPDOProcessor_t *proc)
{
    vcapdolst_object_t *pdo;
    vcapdolst_root_t *r;

    if(!proc) return;
    if(!proc->od_root) return;
    
    _vcaPDOProcessor_disconnectDinfoLinks(proc);

    mylog(LOG_INF, "--- connecting dinfos to PDOs ---\n");
    // first connect HW dinfos
    vcaod_connect_hw_dinfos_to_OD(proc->od_root);
    
    r = &(proc->pdolst_root);
    for(pdo = vcapdolst_first(r); pdo; pdo = vcapdolst_next(r, pdo)) {
        int i;
        vcapdo_mapping_t *m = pdo->mapped_objects;
        if(!m) continue;
        for(i=0; i<pdo->mapped_cnt; i++) {
            unsigned ix, six;
            sui_dinfo_t *d;
            vcaod_object_t *object = m[i].object;
            if(!object) continue;
            ix = object->index;
            six = object->subindex;
            
            mylog(LOG_INF, "Looking for dinfo for object %04x:%02x '%s'.\n", ix, six, object->name);
            d = vcaod_get_dinfo_ref(object, 1);
            if(d) {
                //connect dinfo to rx_hub
                if(!(pdo->flags & (1<<vcapdoFlagRPDO)) && pdo->transmition_type >= 254) {
                    // event driven PDO
                    mylog(LOG_MSG, "Connecting %04x:%02x '%s'-> PDO %04x (type == %i).\n", ix, six, object->name, pdo->cob_id, pdo->transmition_type);
                    sui_dinfo_connect_to_hub(d, &(pdo->rx_hub), NULL);
                    mylog(LOG_DEB, "\ttx hub: %p\n", d->tx_hub);
                }
            }
            m[i].dinfo = d;
        }
    }
    mylog(LOG_TRASH, "vcaPDOProcessor_makeDinfoLinks(): normal EXIT\n");
}
//-----------------------------------------------------------------
int vcaPDOProcessor_processMsg(vcaPDOProcessor_t *proc, canmsg_t *msg)
{
    int ret = -1;
    vcapdolst_object_t *o;

    if(!proc) {
        mylog(LOG_ERR, "vcaPDOProcessor_processMsg(): PDO Processor is NULL\n");
        return ret;
    }

    o = vcapdolst_find(&(proc->pdolst_root), &(msg->id));
    if(!o) return ret;
    
    mylog(LOG_DEB, "PDOprocessor got registered PDO COBID 0x%x\n", msg->id);
    ret = 0;
    return ret;
}
//-----------------------------------------------------------------
//=================================================================
#if 0
//=================================================================
//                  vcaDinfoManger
//=================================================================
void vcaDinfoManager_init(vcaDinfoManager_t *dimgr)
{
}
//-----------------------------------------------------------------
void vcaDinfoManager_destroy(vcaDinfoManager_t *dimgr)
{
}
//-----------------------------------------------------------------
sui_dinfo_t* vcaDinfoManager_getObjectDinfo(vcaDinfoManager_t *dimgr, unsigned ix, int six)
{
    return NULL;
}
//-----------------------------------------------------------------
const char* vcaDinfoManager_getObjectDinfoName(sui_dinfo_t* di)
{
    return "";
}
//=================================================================
#endif