#include "vmodican.h"

#include <iostream.h>      // cout
#include <iomanip.h>       // setw, setfill

#include <fcntl.h>         // O_RDONLY
#include <errno.h>         // errno
#include <unistd.h>        // read
#include <pthread.h>       // pthread_create
#include <sys/ioctl.h>     // ioctl
#include <sys/resource.h>  // PRIO_PROCESS

void VmodIcan::PrintMsg(Message *m)
{
    cout << "Cmd=0x" << hex << (int)m->cmd << dec << " " << flush;
    cout << "len=" << (int)m->len << ":" << flush;

    cout << hex << flush;
    for (int i=0; i<m->len; i++)
        cout << " " << (int)m->data[i] << flush;
    cout << dec << endl;
}

int VmodIcan::Ioctl(int msg, void *arg)
{
    return ioctl(fd, msg, (int)arg) >= 0;
}

void VmodIcan::SetTermination(int state)  /* 0 = off, 1 = on */
{
    /* -*-Func-*-
     *
     * SwitchCanTermination - Switch the CANbus termination resistor
     *
     * The VMOD-ICAN3 module allows the user to change the state of the CANbus
     * termination via software. This is done with this service.
     *
     * SERVICE: HwConf
     */

    Message msg;

    msg.cmd = M_HW_CONF;

    msg.len = 2;
    msg.data[0] = 0x00;
    msg.data[1] = (BYTE_t)state;

    while (!Send(&msg));

    lout << "- CAN Bus Termination set to " << (state?"on":"off") << endl;
}

void *VmodIcan::ReceiveThread(void *data)
{
    VmodIcan *cal = (VmodIcan*)data;

    cal->lout << "- Starting Receiver Loop." << endl;

    //
    // set higher priority to make sure that no messages are lost.
    //
    setpriority(PRIO_PROCESS, 0, -10);

    while (1)
    {
        unsigned char c;
        struct timeval tv;

        //cout << "waiting..." << endl;

        const int n = read(cal->fd, &c, 1); // sleep until message arrive

        gettimeofday(&tv, NULL);

        //cout << "read n=" << n << " c=" << (int)c << endl;

        if (n == 0)
        {
            cerr << "panic read (errno=" << errno << ") ..." << endl;
            return (void *)1;
        }

        switch(c)
        {
        case FAST_QUEUE:
            cout << "--> Fast Queue:  " << flush;

            FastMessage fmsg;

            if (cal->ReceiveFast(&fmsg) < 0)
                return (void *)1;

            cout << "Fast msg ID " <<
                (fmsg.data[0] << 3) + ((fmsg.data[1] >> 5) & 7) << ": " << flush;

            for(int i=0; i<16; i++)
                cout << (int)*(((unsigned char *)(&fmsg))+i) << " " << flush;

            cout << endl;
            break;

        case PLAIN_QUEUE:

            Message msg;

            if (cal->Receive(&msg) < 0)
                return (void *)1;

            //cal->PrintMsg(&msg);
            cal->HandleMessage(&msg, &tv);

            break;
        }

    }
}

void VmodIcan::HandleMessage(Message *msg, struct timeval *tv)
{
    const WORD_t desc  = msg->data[2]<<8 | msg->data[3];
    const BYTE_t rtr   = (desc>>4)&1;
    const BYTE_t len   = desc&0xf;
    const WORD_t cobid = desc>>5;

    switch (msg->cmd)
    {
    case M_MSG_LOST:
        cout << "VmodIcan reports: " << dec << (int)msg->data[0] << " msg(s) lost!" << endl;
        return;

    case M_BCAN_EVENT_ind:
        cout << "VmodIcan reports: CEVTind=0x37, " << hex;
        switch (msg->data[0]) // error indicator
        {
        case 0x01:
            cout << "Error interrup occured" << endl;
            cout << "This means noisy network normally. Please check the bus termination." << endl;
            switch (msg->data[1]) // msg type (board depending)
            {
            case 2: // SJA1000
                cout << dec;
                cout << "ModeReg=" << (int)msg->data[2] << ", ";
                cout << "StatReg=" << (int)msg->data[3] << ", ";
                cout << "RxErrCnt=" << (int)msg->data[4] << ", ";
                cout << "TxErrCnt=" << (int)msg->data[5] << endl;
            }
            TerminateApp();
            return;
        case 0x02:
            cout << "Overrun interrup occured" << endl;
            return;
        case 0x04:
            cout << "Interrupts lost" << endl;
            return;
        case 0x08:
            cout << "Send queue full" << endl;
            return;
        case 0x10:
            cout << "CANbus bus-error" << endl;
            return;
        }
        return;
    case M_BCAN_RX_ind:
        HandleCanMessage(cobid, &msg->data[4], tv);
        return;
    }

    cout << hex;
    cout << "Cmd=0x"    << (int)msg->cmd << ":  ";
    cout << "Descr: 0x" << cobid << dec;
    cout << "  Rtr: "   << (rtr?"Yes":"No");
    cout << "  Len: "   << (int)len << endl;

    cout << "As Raw Data:" << hex << setfill('0');
    for (int i=0; i<msg->len; i++)
        cout << " " << setw(2) << (int)(msg->data[i]) << flush;
    cout << endl;
}

int VmodIcan::Receive(Message *pm) /* receive buffer */
{
    /* can_recv - receive a message from standard interface
     *
     * This function reads a whole message from the standard host interface of
     * a VMOD-ICAN.
     * The module is selected by the module number <fd>.
     *
     * The structure <pm> is filled with the received message.
     *
     * RETURNS:
     * The function returns the number of message received, or -1 when the
     * system call failed.
     * The return value therefore 0 determines, that no message was available to
     * be read: can_recv() does not block in such a case and therefore
     * can be used to poll a module for incoming messages.
     */

    struct dpm_rw_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_READ_MBOX, &arg))
        return -1;

    return arg.rval;
}

int VmodIcan::ReceiveFast(FastMessage *pm)
{

    /* can_recv_fast - receive a message from layer2 interface
     *
     * This function reads a FastMessage from the layer2 fast message
     * interface of a VMOD-ICAN.
     * The module is selected by the file descriptor <fd>.
     *
     * The structure <pm> is filled with the received message.
     *
     * RETURNS:
     * The function returns -1 when the * system call failed.
     * The return value therefore 0 determines, that no message was available to
     * be read: can_recv_fast() does not block in such a case and therefore
     * can be used to poll a module for incoming messages.
     */

    struct dpm_write_fast_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_READ_FAST_MBOX, &arg))
        return -1;

    return arg.rval;
}


void VmodIcan::SetBaudRate(int rate)
{
    /* IcWriteBtrBCAN - Set bit timing parameters
     *
     * Set bit timing parameters in CAN controller. May only be used if
     * CAN controller is in bus-off state. <btr> stores the bus-timing
     * parameters as required by the 82C200 controller. See the description
     * of the CBTRreq-service for possible values.
     *
     * BTR1 is stored in the upper byte of <btr> and BTR0 in the lower. Examples
     * are:
     * .CS
     *     Baudrate   btr       Macro
     *       1Mbit    0x2300    BTR_1MB
     *     500kBit    0x1c00    BTR_500KB
     *     250kBit    0x1c01    BTR_250KB
     *     125kBit    0x1c03    BTR_125KB
     *     100kBit    0x34c7    BTR_100KB
     *      50kBit    0x34cf    BTR_50KB
     *      20kBit    0x7fcf    BTR_20KB
     * .CE
     *
     * SERVICE: CBTRreq
     *
     * NOTE:
     * Raw ICANOS version of the firmware only.
     */

    /* create message for vmod-ican */
    Message msg;  /* buffer for module messages */

    int rateid;

    switch (rate)
    {
    case 1000:
        rateid=BTR_1MB;
        break;
    case  500:
        rateid=BTR_500KB;
        break;
    case  250:
        rateid=BTR_250KB;
        break;
    case  125:
        rateid=BTR_125KB;
        break;
    case  100:
        rateid=BTR_100KB;
        break;
    case   50:
        rateid=BTR_50KB;
        break;
    case   20:
        rateid=BTR_20KB;
        break;

    default:
        cout << "Error: Wrong bit rate specified" << endl;
        return;
    }

    msg.cmd = M_BCAN_SET_BTR_req;

    msg.len = 4;

    msg.data[2] = word_to_lsb(rateid);
    msg.data[3] = word_to_msb(rateid);

    while (!Send(&msg));      /* transmitt to module */

    lout << "- Baudrate set to " << rate << "bps" << endl;
}


void VmodIcan::EnableCanBusConnection()
{
    /* IcBusOnBCAN - switch CANbus controller to bus-on state
     *
     * Switch CAN controller bus-on. You will need to use this
     * function explicitly after you have connected yourself
     * to the module with can_open() (or ican_open() under DOS/WINDOWS).
     * This is because the module comes up in the bus-off state.
     *
     * SERVICE: CONreq
     *
     * NOTE:
     * Raw ICANOS version of the firmware only.
     */
    Message msg;                  /* buffer for module messages */

    msg.cmd = M_BCAN_BUSON_req;
    msg.len = 0;

    while (!Send(&msg));

    lout << "- Controller connected to bus" << endl;
}

int VmodIcan::EnableFastCan(int rbuffers, int wbuffers)
{
    /* ican2_init_fast_can - initialize fast can access for VMOD-ICAN
     *
     * By this function, the user may initialize and enable the fast
     * host interface (layer2 access) for a VMOD-ICAN module.
     *
     * The calling application can request <rbuffers> buffer elements in the queue
     * that sends data to the host and <wbuffers> buffer elements for the queue
     * that transports data to the module.
     *
     * NOTE:
     * Notice that the message filtering on the VMOD-ICAN has to be
     * set correctly, so that messages can be received through the fast
     * interface.
     *
     * CAVE AT:
     * The <rbuffers> and wbuffers> have no special limit, but the general
     * resources of the DPM must not be exceeded.
     * For the calculation you need to assume, that 16 buffers in one of the fast
     * interface queues take the same DPM space as 1 buffer in the standard
     * host interface.
     *
     * The user must use only one of the functions, either
     * ican2_init_fast_can or ican2_init_fast_can_prio
     *
     * RETURNS:
     * Zero if the operation performed successfully, or less than zero on error.
     */
    struct dpm_fast_can_desc hdp;

    hdp.tohost_len   = rbuffers;
    hdp.fromhost_len = wbuffers;

    if (!Ioctl(DPM_INIT_FAST_CAN, &hdp))
        return -1;

    lout << "- Fast Host Interface Enabled" << endl;

    return 0;
}

void VmodIcan::DisableCanBusConnection()
{
    /* IcWriteEwlBCAN - Set error warning limit
     *
     * Set error warning limit in CAN controller. If this limit is passed, the
     * user will get a CEVTind message stating an error interrupt. This type
     * of message will also occur if the both error counter again fall below
     * this limit.
     *
     * RESTRICTIONS:
     * Will only take effect if CAN controller is in bus-off state. Requires
     * an SJA1000 CANbus controller, and will be no-op for 82C200.
     *
     * SERVICE: CBCONFreq
     *
     * NOTE:
     * Raw ICANOS version of the firmware only.
     */
    lout << "- Disconnect from Bus!" << endl;

    Message msg;                  /* buffer for module messages */

    msg.cmd = M_BCAN_BUSOFF_req;
    msg.len = 0;

    while (!Send(&msg));
}

void VmodIcan::Close()
{
    /* can_close - close connection to a VMOD-ICAN module
     *
     * The function can be used to close a connection to a VMOD-ICAN
     * that has been established by a can_open() call.
     * The module has to be selected by the file descriptor <fd> which was
     * obtained when you did the can_open() call.
     *
     * When you call can_close, all the resources that were used by the driver
     * for communication are freed.
     *
     * The VMOD-ICAN module under question will be reseted, to make sure that
     * the communication with the host will stop. That means especially that
     * no further interrupt will occur and that the module will not longer be
     * active on the CANbus.
     *
     * RETURNS: N/A
     */
    lout << "- Close Device!" << endl;

    Message msg; /* disconnect message */

    msg.cmd = M_DISCONNECT;
    msg.len = 0;

    while (!Send(&msg));

    close(fd);
}

int VmodIcan::EnableFifo()
{
    //
    // Allow VMOD to send messages through the fifo
    //
    Message msg; /* connect message */

    msg.cmd = M_CONNECT_INTR;
    msg.len = 0;

    while (!Send(&msg));

    lout << "- Fifo enabled" << endl;

    return TRUE;
}

int VmodIcan::Reset()
{
    const int rc = Ioctl(DPM_RESET, 0);

    lout << "- Reset done." << endl;

    return rc;
}

int VmodIcan::Open(const char *devname)          /* pathname of device */
{
    /* can_open - open VMOD-ICAN device
     *
     * With this function call you open a VMOD-ICAN plugged
     * into a MODULbus carrier board for use. The module is
     * reseted and then initialized for communication to the host.
     *
     * A specific module is selected by it's device name (e.g. "/dev/dpm_01").
     */

    fd = open (devname, O_RDONLY, 0);

    if (fd < 0)
    {
        lout << "Error: Opening device '" << devname << "' (rc=" << fd << ")" << endl;
        return FALSE;
    }

    lout << "- Device " << devname << " open." << endl;

    return TRUE;
}

int VmodIcan::StdHost2NewStyle(int rbuffers, int wbuffers,
                               int wbuffers_hi, int wbuffers_low)
/* buffers in to, from, from_hi, from_low host queue */
/* ican2_select_hostif - switch standard host interface to new style mode
 *
 * The routine ican2_select_hostif() can be used to switch a module from
 * the standard host interface to the new style mode. The module is selected
 * by the module number <fd>.
 *
 * The calling application can request <rbuffers> buffer for the communication
 * queue that sends data to the host and <wbuffers> buffer for the reverse
 * communication direction (normal priority queue). By this function the hi- and
 * low-prioritized message-queues which sends data to the module are initialized
 * to a length of 1.
 *
 * NOTE:
 * To notify the module of the new situation, the driver sends
 * a M_NEWHOSTIF message to the module. This is the last message to be
 * transfered through the old style host interface. Immediately after
 * sending this message, the library is switched to the new style mode.
 * Any messages that are sent by the module in this time gap, may be lost.
 * It is therefore not recommended to use this function when you wait
 * for messages from the module.
 *
 * The selection of the new host interface is not reversible. It will stay
 * until the next reset for the module occurs. This will probably occur
 * when you use can_close().
 *
 * HINTS:
 * When the new style mode is active, no more internal message buffering
 * on the module exists. That is whenever the module tries to send something
 * and cannot because the queue is full, this message will be dropped.
 * Thereby, when enabling the new style host interface you should create
 * enough buffers for the queue that sends to the host, to prevent the
 * loss of messages. If you loose messages, however you will be indicated
 * of that event by a MSGLOST messages (which will not be lost!).
 *
 * CAVE AT:
 * The parameters <rbuffers>, <wbuffers>, <wbuffers_hi> and <wbuffers_low>
 * must be greater than 0, less than 128, and the total sum must not
 * exceed 236. These parameters aren't checked by the driver!
 */
{
    struct dpm_new_hostif_desc_prio hdp;

    hdp.tohost_len       = rbuffers;
    hdp.fromhost_len     = wbuffers;
    hdp.fromhost_hi_len  = wbuffers_hi;
    hdp.fromhost_low_len = wbuffers_low;

    Ioctl(DPM_INIT_NEW_HOSTIF_PRIO, &hdp);

    lout << "- New style host interface enabled" << endl;

    return 0;
}

int VmodIcan::SendHi(Message *pm)
{
    /* can_send_hi - send message to standard host interface (high priority)
     *
     * This function performs the same action as can_send(), except it will
     * append message <pm> to the highest priority queue of the standard
     * host interface.
     *
     * NOTE:
     * Notice that the prioritized issue of the message take effect on the new style
     * mode of the standard host interface only.
     *
     * RETURNS:
     * The function returns the number of message send, or -1 when the system
     * call failed.
     * The return value 0 determines that no message could be send,
     * probably because there was no space in the targeted queue. can_send_hi()
     * does not block or retry in such a case, so you need to loop explicitly
     * until the message is send.
     */
    struct dpm_rw_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_WRITE_MBOX_HI, &arg))
        return FALSE;

    return arg.rval;
}

int VmodIcan::SendLo(Message *pm)
{
    /* can_send_low - send message to standard host interface (low priority)
     *
     * This function performs the same action as can_send(), except it will
     * append message <pm> to the lowest priority queue of the standard
     * host interface.
     *
     * NOTE:
     * Notice that the prioritized issue of the message take effect on the new
     * style mode of the standard host interface only.
     *
     * RETURNS:
     * The function returns the number of message send, or -1 when the system
     * call failed.
     * The return value 0 determines that no message could be send,
     * probably because there was no space in the targeted queue. can_send_low()
     * does not block or retry in such a case, so you need to loop explicitly
     * until the message is send.
     *
     */
    struct dpm_rw_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_WRITE_MBOX_LOW, &arg))
        return FALSE;

    return arg.rval;
}

int VmodIcan::Send(Message *pm) /* file descriptor, message to send */
{
    /* can_send - send message to standard host interface (mid priority)
     *
     * This function sends a complete message to the standard host interface of
     * a VMOD-ICAN.
     *
     * The message <pm> will be queued to the middle prioritized of the three
     * queues.
     *
     * RETURNS:
     * The function returns the number of message send, or -1 when the system
     * call failed.
     * The return value 0 determines that no message could be send,
     * probably because there was no space in the targeted queue. can_send()
     * does not block or retry in such a case, so you need to loop explicitly
     * until the message is send.
     */
    struct dpm_rw_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_WRITE_MBOX, &arg))
        return FALSE;

    return arg.rval;
}

int VmodIcan::Send(FastMessage *pm) /* file descriptor, message to send */
{
    /* can_fast_send - send message to fast interface
     *
     * This function sends a message to the fast host interface (layer-2
     * interface) of a VMOD-ICAN. The module is selected by the module number
     * <fd>.
     * The message to be send will be given in the structure <pm>.
     *
     * The fast host interface needs to be established before can_fast_send()
     * can be used successfully.
     *
     * RETURNS:
     * The function returns 1 if can_fast_send() completed successful.
     * Otherwise the return value 0 determines that the message could not be send,
     * probably because there was no space in the DPM. The function
     * does not block or retry in such a case, so you need to loop explicitly
     * until the message is send.
     * The function returns -1 when the system-call itself failed.
     */
    struct dpm_write_fast_can_desc arg;

    arg.pm = pm;

    if (!Ioctl(DPM_WRITE_FAST_CAN, &arg))
        return FALSE;

    lout << "done." << endl;

    return arg.rval;
}

void VmodIcan::DisableAllCobIds()
{
    /* -*-Func-*-
     *
     * IcSetAfil - Set software acceptance filter mask
     *
     * Set software acceptance filtering.
     *
     * SERVICE: SetAfilMask
     */
    Message msg;

    msg.cmd = M_SET_AFIL;
    msg.len = 5;

    msg.data[0]  = word_to_lsb(0);
    msg.data[1]  = word_to_msb(0);
    msg.data[2]  = word_to_lsb(0x7ff); // 11 bit Cob-Ids
    msg.data[3]  = word_to_msb(0x7ff); // 11 bit Cob-Ids
    msg.data[4]  = 0;

    while (!Send(&msg));

    lout << "- All CobIds disabled." << endl;
}


void VmodIcan::EnableCobId(WORD_t cobid, int flag)
{
    /* -*-Func-*-
     *
     * IcSetAfil - Set software acceptance filter mask
     *
     * Set software acceptance filtering.
     *
     * SERVICE: SetAfilMask
     *
     * NOTE: This service is available in both version of the firmware: Raw-CAN
     * and CAL.
     */
    Message msg;

    msg.cmd = M_SET_AFIL;
    msg.len = 3;

    msg.data[0]  = word_to_lsb(cobid);
    msg.data[1]  = word_to_msb(cobid);
    msg.data[2]  = (BYTE_t)(flag?3:0);

    while (!Send(&msg));

    lout << "- CobId 0x" << hex << setfill('0') << setw(3) << cobid << " enabled." << endl;
}

void VmodIcan::SendCanFrame(WORD_t cobid, BYTE_t m[8])
{
       /* -*-Func-*-
         *
         * IcSendReqBCAN - Send a CANbus message
         *
         * Issue request to send a CAN message. <Spec> controls whether to send with
         * or without spec/confirmation.
         * .CS
         *    spec     action
         *      0      send only
         *      1      send with confirmation to the host.
         *      2      send and echo message to the host.
         *      3      send and generate both echo and confirmation.
         * .CE
         *
         * SERVICE: CTXreq, CTXCreq, CTXEreq, CTXCEreq
         *
         * NOTE:
         * Raw ICANOS version of the firmware only.
         */
        const WORD_t desc = MsgDescr(cobid, 8);

        Message msg;

	msg.cmd = M_BCAN_TX_req;

        msg.len = 12;
	msg.data[0]  = 0;
	msg.data[1]  = 0;
	msg.data[2]  = word_to_msb(desc);
        msg.data[3]  = word_to_lsb(desc);

	memcpy(&msg.data[4], m, 8);

        while (!Send(&msg));

        /*
        cout << "0x" << hex << (int)cobid << ": ";
        for(int i=0; i<10; i++)
            cout << hex << (int)msg.data[i+2] << " " << flush;
        cout << endl;
        cout << "- Message sent." << endl;
        */

}

VmodIcan::VmodIcan(const char *dev, const int baud, ostream &out) : Log(out), fRxThrd(NULL)//: CanDriver(dev, baud)
{
    Open(dev);                         // open module
    Reset();
    EnableFifo();                      // connect to host (in interrupt mode)
    SetBaudRate(baud);                 // set baud rate
    DisableAllCobIds();
    EnableCanBusConnection();          // connect to bus
    /*
     StdHost2NewStyle(50, 50, 50, 50);  // set host style
     EnableFastCan(50, 50);
     SetTermination(0);
    */
}

VmodIcan::~VmodIcan()
{
    StopReceiver();
    DisableCanBusConnection();
    Close();
}

void VmodIcan::SendCanFrame(WORD_t cobid,
                            BYTE_t m0, BYTE_t m1, BYTE_t m2, BYTE_t m3,
                            BYTE_t m4, BYTE_t m5, BYTE_t m6, BYTE_t m7)
{
    BYTE_t msg[8] = { m0, m1, m2, m3, m4, m5, m6, m7 };
    SendCanFrame(cobid, msg);
}

void VmodIcan::StopReceiver()
{
    if (!fRxThrd)
        return;

    pthread_cancel(*fRxThrd);

    delete fRxThrd;
    fRxThrd = NULL;

    lout << "- Receiver Thread stopped." << endl;
}
void VmodIcan::StartReceiver()
{
    //**************************************
    //* create thread waiting for messages *
    //**************************************
    if (fRxThrd)
    {
        cout << "Error: rx thread already started." << endl;
        return;
    }

    lout << "- Starting receiving Thread." << endl;

    fRxThrd = new pthread_t;
    pthread_create(fRxThrd, NULL, ReceiveThread, this);
}