/* ======================================================================== *\
!
! *
! * This file is part of Stesy, the MAGIC Steering System
! * Software. It is distributed to you in the hope that it can be a useful
! * and timesaving tool in analysing Data of imaging Cerenkov telescopes.
! * It is distributed WITHOUT ANY WARRANTY.
! *
! * Permission to use, copy, modify and distribute this software and its
! * documentation for any purpose is hereby granted without fee,
! * provided that the above copyright notice appear in all copies and
! * that both that copyright notice and this permission notice appear
! * in supporting documentation. It is provided "as is" without express
! * or implied warranty.
! *
!
!
!   Author(s): Thomas Bretz <mailto:tbretz@astro.uni-wuerzburg.de>, 2003
!
!   Copyright: MAGIC Software Development, 2000-2008
!
!
\* ======================================================================== */

///////////////////////////////////////////////////////////////////////
//
// CanOpen
//
// implements the canopen layer over the raw device driver
//
///////////////////////////////////////////////////////////////////////
#include "canopen.h"

#include "MLog.h"
#include "MLogManip.h"

#include "interface.h"

ClassImp(CanOpen);

using namespace std;

// --------------------------------------------------------------------------
//
// Initializes a conditional and a mutex semaphore for all possible
// PDO combinations
//
CanOpen::CanOpen() : fInterface(0)
{
    gLog << inf << "- CanOpen initialized." << endl;
}

// --------------------------------------------------------------------------
//
// Destroys all conditional and mutex semaphores
//
CanOpen::~CanOpen()
{
    gLog << inf << "- CanOpen destroyed." << endl;
}

// --------------------------------------------------------------------------
//
// Start the interface
//
void CanOpen::Start()
{
    if (fInterface)
        fInterface->Start();
}

// --------------------------------------------------------------------------
//
// Stop the interface
//
void CanOpen::Stop()
{
    if (fInterface)
        fInterface->Stop();
}

// --------------------------------------------------------------------------
//
// This overloads Interface::HandleCanMessage. It is called if a can
// message was received with all message relevant data (COBId, data, time)
// It distributes the data depending on its function code to several
// functions (to be obverloaded)
// In case of a PDO the conditional semaphore corresponding to this PDO
// is raised (WaitForNextPDO)
//  HandleSDO:       handles a SDO message
//  HandlePDO1/2/3/4:handles received PDOs
//
void CanOpen::HandleCanMessage(WORD_t cobid, const BYTE_t *data, const timeval_t &tv)
{
    const WORD_t fcode = cobid >> 7;
    const BYTE_t  node = cobid & 0x1f;

    switch (fcode)
    {
    case kNMT:
        cout << "NMT: " << hex ;
        cout << "CobId: 0x" << cobid << " ";
        cout << "cmd=0x" << (int)data[0] << " ";
        cout << "node=" << dec << (int)data[1] << endl;
        return;

    case kEMERGENCY: // also case kSYNC:
        if (cobid==0)
            cout << "Sync" << endl;
        else
        {
            cout << "EMERGENCY Node #" << dec << (int)data[1] << endl;
            HandleEmergency(node, tv);
        }
        return;

    case kNodeguard:
        //cout << "Nodeguard Node #" << dec << (int)node << endl;
        HandleNodeguard(node, tv);
        return;

    case kSDO_RX:
        {
            const BYTE_t  cmd    = data[0];
            const LWORD_t dat    = data[4] | (data[5]<<8) | (data[6]<<16) | (data[7]<<24);
            const WORD_t  idx    = data[1] | (data[2]<<8);
            const WORD_t  subidx = data[3];

            cout << "SND NODE: " << (int)node << " " << flush;
            HandleSDO(node, cmd, idx, subidx, dat, tv);

            fSdoList.Del(node, idx, subidx);
        }
        return;

    case kPDO1_TX:
        {
            HandlePDO1(node, data, tv);
            fPdoCond[node-1][0].Broadcast();
        }
        return;

    case kPDO2_TX:
        {
            HandlePDO2(node, data, tv);
            fPdoCond[node-1][1].Broadcast();
        }
        return;

    case kPDO3_TX:
        {
            HandlePDO3(node, data, tv);
            fPdoCond[node-1][2].Broadcast();
        }
        return;

    case kPDO4_TX:
        {
            HandlePDO4(node, data, tv);
            fPdoCond[node-1][3].Broadcast();
        }
        return;
    }

    cout << "CanOpen::HandleCanMessage - Unhandled Message: Function Code=0x" << hex << fcode << "  Node #" << dec << (int)node << endl;
}

// --------------------------------------------------------------------------
//
// Does a basic message processing and hadles the so called command (cmd)
// stamp of the message. This is some kind of message type which is send
// by the can interface
//
void CanOpen::HandleMessage(const Message &msg, const timeval_t &tv)
{
    //
    // Decode message
    //
    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) // FROM mican.h
    {
    case M_MSG_LOST:
        cout << "Interface reports: " << dec << (int)msg.data[0] << " msg(s) lost!" << endl;
        return;

    case M_BCAN_TX_con:  /* confirm (+/-) transmission */
        cout << "Interface reports: CTXcon=0x35" << endl;
        cout << "This normally means, that the transmission of the following CAN frame failed:" << hex << endl;
        cout << "Descr: 0x" << cobid << dec;
        cout << "  Rtr: "   << (rtr?"Yes":"No");
        cout << "  Len: "   << (int)len << endl;
        return;

    case M_BCAN_EVENT_ind:
        cout << "Interface 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;
            }
            //FIXME? 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:
        //
        // Message is a message from the Can bus
        //
        //cout << "HandleCanMessage " << cobid << endl;
        HandleCanMessage(cobid, &msg.data[4], tv);
        return;
    }

    //
    // Nothing of the above happened
    //
    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;
}

bool CanOpen::WaitForSdos(WORDS_t ms)
{
    // 0: unlimited
    // <0: don't do a real wait.
    if (ms<0)
    {
        fSdoList.DelAll();
        return true;
    }

    MTimeout t(ms);
    while (fSdoList.IsPending() &&
           !StopWaitingForSDO() &&
           !t.HasTimedOut())
        usleep(1);

    bool rc = true;
    if (ms && t.HasTimedOut())
    {
        cout << "WaitForSdos timed out." << endl;
        rc = false;
    }
    /*
     if (StopWaitingForSDO())
     {
     cout << "WaitForSdos stopped." << endl;
     rc = false;
     }
     */
    if ((ms && t.HasTimedOut()) || StopWaitingForSDO())
        fSdoList.DelAll();

    return rc;
}

bool CanOpen::WaitForSdo(BYTE_t node, WORD_t idx, BYTE_t subidx, WORDS_t ms)
{
    // 0: unlimited
    // <0: don't do a real wait.

    if (ms<0)
    {
        fSdoList.Del(node, idx, subidx);
        return true;
    }

    MTimeout t(ms);
    while (fSdoList.IsPending(node, idx, subidx) &&
           !StopWaitingForSDO() &&
           !t.HasTimedOut())
        usleep(1);

    bool rc = true;
    if (ms && t.HasTimedOut())
    {
        cout << "WaitForSdo Node #" << (int)node << " " << idx << "/" << (int)subidx << " timed out." << endl;
        rc = false;
    }
    /*
     if (StopWaitingForSDO())
     {
     cout << "WaitForSdo Node #" << (int)node << " " << idx << "/" << (int)subidx << " stopped." << endl;
     rc = false;
     }
     */
    if ((ms && t.HasTimedOut()) || StopWaitingForSDO())
        fSdoList.Del(node, idx, subidx);

    return rc;
}

// --------------------------------------------------------------------------
//
// Enables can messaged for a given node ID and function code.
//
void CanOpen::EnableCanMsg(BYTE_t node, BYTE_t fcode, int flag)
{
    if (node>=0x20)
        return;

    if (fInterface)
        fInterface->EnableCobId(CobId(node, fcode), flag);
}

// --------------------------------------------------------------------------
//
// Enables Emergency messages for a given node
//
void CanOpen::EnableEmcy(BYTE_t node)
{
    EnableCanMsg(node, kEMERGENCY);
}

// --------------------------------------------------------------------------
//
// Enables Nodeguard messages for a given node
//
void CanOpen::EnableNodeguard(BYTE_t node)
{
    EnableCanMsg(node, kNodeguard);
}


// --------------------------------------------------------------------------
//
// Enables SDO rx messages for a given node
//
void CanOpen::EnableSdoRx(BYTE_t node)
{
    EnableCanMsg(node, kSDO_RX);
}

// --------------------------------------------------------------------------
//
// Enables PDO1 tx messages for a given node
//
void CanOpen::EnablePdo1Rx(BYTE_t node)
{
    EnableCanMsg(node, kPDO1_TX);
}

// --------------------------------------------------------------------------
//
// Enables PDO2 tx messages for a given node
//
void CanOpen::EnablePdo2Rx(BYTE_t node)
{
    EnableCanMsg(node, kPDO2_TX);
}

// --------------------------------------------------------------------------
//
// Enables PDO3 rx messages for a given node
//
void CanOpen::EnablePdo3Rx(BYTE_t node)
{
    EnableCanMsg(node, kPDO1_TX);
}

// --------------------------------------------------------------------------
//
// Enables PDO4 rx messages for a given node
//
void CanOpen::EnablePdo4Rx(BYTE_t node)
{
    EnableCanMsg(node, kPDO2_TX);
}

// --------------------------------------------------------------------------
//
// Sends a PDO1 message with the given data to the given node
//
void CanOpen::SendPDO1(BYTE_t node, BYTE_t data[8])
{
    SendCanFrame(CobId(node, kPDO1_TX), data);
}

// --------------------------------------------------------------------------
//
// Sends a PDO2 message with the given data to the given node
//
void CanOpen::SendPDO2(BYTE_t node, BYTE_t data[8])
{
    SendCanFrame(CobId(node, kPDO2_TX), data);
}

// --------------------------------------------------------------------------
//
// Sends a PDO3 message with the given data to the given node
//
void CanOpen::SendPDO3(BYTE_t node, BYTE_t data[8])
{
    SendCanFrame(CobId(node, kPDO3_TX), data);
}

// --------------------------------------------------------------------------
//
// Sends a PDO4 message with the given data to the given node
//
void CanOpen::SendPDO4(BYTE_t node, BYTE_t data[8])
{
    SendCanFrame(CobId(node, kPDO4_TX), data);
}

// --------------------------------------------------------------------------
//
// Sends a PDO1 message with the given data to the given node
//
void CanOpen::SendPDO1(BYTE_t node,
              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(node, kPDO2_TX), msg);
}

// --------------------------------------------------------------------------
//
// Sends a PDO2 message with the given data to the given node
//
void CanOpen::SendPDO2(BYTE_t node,
              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(node, kPDO2_TX), msg);
}

// --------------------------------------------------------------------------
//
// Sends a PDO3 message with the given data to the given node
//
void CanOpen::SendPDO3(BYTE_t node,
              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(node, kPDO3_TX), msg);
}

// --------------------------------------------------------------------------
//
// Sends a PDO4 message with the given data to the given node
//
void CanOpen::SendPDO4(BYTE_t node,
              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(node, kPDO4_TX), msg);
}
  
// --------------------------------------------------------------------------
//
// Sends a SDO message with the given data to the given node:
//  - index describing the dictionary index to set
//  - subindex describing the dictionary subindex of theindex to set
//  - val describing the value to set.
//  - store describes whether the sdo should be stored in a list to
//    be able to wait for an answer
//
void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, BYTE_t val, bool store)
{
    if (store)
        fSdoList.Add(node, idx, subidx);

    SendCanFrame(CobId(node, kSDO_TX), kSDO_RX1,
                 word_to_lsb(idx), word_to_msb(idx), subidx, val);
}

// --------------------------------------------------------------------------
//
// Sends a SDO message with the given data to the given node:
//  - index describing the dictionary index to set
//  - subindex describing the dictionary subindex of theindex to set
//  - val describing the value to set.
//  - store describes whether the sdo should be stored in a list to
//    be able to wait for an answer
//
void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, WORD_t val, bool store)
{
    if (store)
        fSdoList.Add(node, idx, subidx);

    SendCanFrame(CobId(node, kSDO_TX), kSDO_RX2,
                 word_to_lsb(idx), word_to_msb(idx), subidx,
                 word_to_lsb(val), word_to_msb(val));
}

// --------------------------------------------------------------------------
//
// Sends a SDO message with the given data to the given node:
//  - index describing the dictionary index to set
//  - subindex describing the dictionary subindex of theindex to set
//  - val describing the value to set.
//  - store describes whether the sdo should be stored in a list to
//    be able to wait for an answer
//
void CanOpen::SendSDO(BYTE_t node, WORD_t idx, BYTE_t subidx, LWORD_t val, bool store)
{
    if (store)
        fSdoList.Add(node, idx, subidx);

    SendCanFrame(CobId(node, kSDO_TX), kSDO_RX4,
                 word_to_lsb(idx), word_to_msb(idx), subidx,
                 word_to_lsb(val&0xffff), word_to_msb(val&0xffff),
                 word_to_lsb(val>>16), word_to_msb(val>>16));
}

// --------------------------------------------------------------------------
//
// Request a SDO message from the given node:
//  - index describing the dictionary index to request
//  - subindex describing the dictionary subindex of the index to request
//
void CanOpen::RequestSDO(BYTE_t node, WORD_t idx, BYTE_t subidx)
{
    fSdoList.Add(node, idx, subidx);

    SendCanFrame(CobId(node, kSDO_TX), kSDO_RX_DATA, word_to_lsb(idx), word_to_msb(idx), subidx);
}

// --------------------------------------------------------------------------
//
// Send a NMT Message to the given node with command cmd
//
void CanOpen::SendNMT(BYTE_t node, BYTE_t cmd)
{
    SendCanFrame(CobId(0, kNMT), cmd, node);
}

// --------------------------------------------------------------------------
//
// Send a Nodeguard Message to the given node with command cmd
//
void CanOpen::SendNodeguard(BYTE_t node)
{
    BYTE_t msg[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };
    SendCanFrame(CobId(node, kNodeguard), msg, 1);
}

// --------------------------------------------------------------------------
//
// This is IcSendReqBCAN from the Janz software
//
//  /*
//   * 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.
//   */
//
void CanOpen::SendCanFrame(WORD_t cobid, BYTE_t m[8], BYTE_t rtr)
{
    if (fInterface)
        fInterface->SendCanFrame(cobid, m, rtr);
}

// --------------------------------------------------------------------------
//
// Sends a can frame with the given cobid and the given eight bytes
// through the can network
//
void CanOpen::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);
}

// --------------------------------------------------------------------------
//
// Decodes node and function code into a CobId
//
WORD_t CanOpen::CobId(BYTE_t node, BYTE_t fcode) const
{
    return (fcode<<7) | node&0x1f;
}