/* ======================================================================== *\
!
! *
! * 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-2003
!
!
\* ======================================================================== */

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

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

ClassImp(CanOpen);

using namespace std;

// --------------------------------------------------------------------------
//
// Initializes a conditional and a mutex semaphore for all possible
// PDO combinations
//
CanOpen::CanOpen(const char *dev, const int baud, MLog &out) : VmodIcan(dev, baud, out)
{
    lout << "- CanOpen initialized." << endl;
}

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

// --------------------------------------------------------------------------
//
// This overloads VmodIcan::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, BYTE_t *data, 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];

            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;
}

// --------------------------------------------------------------------------
//
// 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;

    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=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
              BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
{
    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=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
              BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
{
    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=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
              BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
{
    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=0, BYTE_t m1=0, BYTE_t m2=0, BYTE_t m3=0,
              BYTE_t m4=0, BYTE_t m5=0, BYTE_t m6=0, BYTE_t m7=0)
{
    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);
}

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