/**
 * A sample TLM which uses the SoftPLC CANBUS API
 */

/*
BECAUSE THE PROGRAM IS SUBJECT TO CHANGE BY LICENSEE, THERE IS NO WARRANTY
FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW.  EXCEPT WHEN
OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES
PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED
OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.  THE ENTIRE RISK AS
TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU.  SHOULD THE
PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING,
REPAIR OR CORRECTION.

IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING
WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR
REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES,
INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING
OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED
TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY
YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER
PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE
POSSIBILITY OF SUCH DAMAGES.
*/



#include <softplc_toolkit.h>
#include <can_bus.h>

DECLARE_TLM( TLM, "TEST", 0 )

DT_INT16    can_port_status( "CAN_PORT_STATUS" );
DT_INT16    can_port_too_busy( "CAN_PORT_TOO_BUSY" );

CAN_PORT    can_port( "can0" );
CAN_MSG     rx_msg;
CAN_MSG     tx_msg;

// Multiple state machines are possible, here we use only one named "timer".

//-----<FSM timer>---------------------------------------------------------------

enum FSM1_STATES
{
    FSM1_STATE1,
    FSM1_STATE2,
};
FSM timer( "timer_state" ); // "timer_state" is an integer word Tag in Descriptor Table


void TLM::Init( req_init* req )
{
    printf( "%s: " __DATE__ " initializing...\n", Name() );

    std::string result = can_port.Open();

    if( result.size() )
        throw ERROR( can_port.IsErrored(),
            "%s",
            result.c_str()          // text returned from Open()
            );

    // the throw ERROR if it happens, prints a message on the SoftPLC console
    // or into the syslog.

#if 0
    CAN_FILTERS rfilters;

    // always add (CAN_EFF_FLAG | CAN_RTR_FLAG) to can_mask for optimization.
    // https://www.kernel.org/doc/Documentation/networking/can.txt, section 4.1.1.1

    // note also CAN_INV_FILTER in can_bus.h

    // an EFF filter:
    rfilters.push_back(
        CAN_FILTER(
            0x14020101 | CAN_EFF_FLAG,               // id
            0x1fffffff | CAN_EFF_FLAG | CAN_RTR_FLAG // mask
            )
        );

    can_port.SetRecvFilters( rfilters );

#else
    // without setting filter(s), the CAN_PORT will receive everything
    // by default.
#endif

    // We prepare most of the tx_msg payload here, since there is
    // no performance penalty in TLM::Init().  Update dynamic parts later
    // just before CAN_PORT::Send()-ing.
    tx_msg.CanId( 0x14020101 | CAN_EFF_FLAG );  // CAN_EFF_FLAG causes 29 bit ID.

    tx_msg.push_back( 0x10 );
    tx_msg.push_back( 0x20 );
    tx_msg.push_back( 0x30 );
    tx_msg.push_back( 0x40 );
    tx_msg.push_back( 0x50 );
    tx_msg.push_back( 0x60 );
    tx_msg.push_back( 0x70 );
    tx_msg.push_back( 0x80 );

    D(printf("~%s:\n", Name() );)

    // There are now 8 bytes in tx_msg's std::vector.
}


void TLM::FirstScan()
{
    printf( "%s: First Run Mode Scan, re-setting variables that need it.\n",
        Name() );

    timer.SetState( FSM1_STATE1 );

    can_port_too_busy = 0;
}


/**
 * Function parse_packet
 * extracts whatever is currently in global rx_msg and puts it into the
 * datatable using DT_OBJECTs.  CAN_MSG::Format() is useful for debugging.
 */
void parse_packet()
{
#if defined(DEBUG)      // true in Debug build
    printf( "%s: %s\n", __func__, rx_msg.Format().c_str() );
#endif

    if( rx_msg.size() < 8 )
    {
        printf( "%s: CAN packet too short\n", TLM___->Name() );
        return;
    }

    PLCINT w1 = U16GetLE( &rx_msg[0] );    // payload bytes 0 & 1, little endian
    PLCINT w2 = U16GetLE( &rx_msg[2] );    // payload bytes 2 & 3
    PLCINT w3 = U16GetLE( &rx_msg[4] );
    PLCINT w4 = U16GetLE( &rx_msg[6] );

    // parse each CanId separately, strip off the extended format bit first
    switch( CAN_EFF_MASK & rx_msg.CanId() )
    {
    case 0x14020101:
        // assign w1-w4 to specific four DT_INT16s here.
        break;

    case 0x14030101:
        // assign to specific four DT_INT16s here.
        break;

    case 0x140d0101:
        break;

    case 0x140e0101:
        break;

    case 0x140f0101:
        break;

    case 0x14100101:
        break;

    case 0x14110101:
        break;
    }
}


void TLM::Scan()
{
    //-----<CAN_PORT Service>----------------------------------------------------
    // limit the time spent in the following loop.  If inbound CAN traffic
    // consistently exceeds this then we should install some filtering into the
    // CAN_PORT.
    const int work_limit = 200;
    int work;

    for( work=0;  work < work_limit;  ++work )
    {
        int count = can_port.Recv( &rx_msg, 0 );

        if( count > 0 )
        {
            parse_packet();
            can_port_status = 0;        // CAN_PORT is OK
        }
        else if( count < 0 )
        {
            printf( "%s: can_port status:%d\n",
                Name(),               // name of this TLM
                can_port.IsErrored()
                );

            // Notify the Ladder Program and HMI of this CAN_PORT problem.
            // A non-zero value in can_port_status will indicate an error.
            can_port_status = can_port.IsErrored();
        }
        else    // count == 0, timeout & no more packets
            break;
    }

    if( work == work_limit )
    {
        // If this is consistently incrementing, then we know to add
        // filters to our CAN_PORT in order to limit unwanted traffic.
        // Or make efforts to shorten the overall scan interval.
        ++can_port_too_busy;
    }

    //-----<FSM timer>-----------------------------------------------------------
    if( timer.DwellTime() >= 200000 )   // 200 msecs = 200000 usecs
    {
        // should modify the tx_msg payload here with current datatable stuff.
        // ...

        can_port.Send( tx_msg );

        // The state transition itself.  All calls to SetState() reset the dwell timer.
        timer.SetState( timer.State() == FSM1_STATE1 ? FSM1_STATE2 : FSM1_STATE1 );
    }

    // return soon, always
}


// This is called in OPM_PROG or OPM_FAULTED modes only (non-run modes).
// DT_OBJECTs are off limits in this function, because in non-run modes DT_OBJECTs
// are not necessarily resolved.  Using them from TLM::Idle() will usually
// cause a SegFault, or worse, corrupt memory.
void TLM::Idle()
{
    // Keep the CAN port drained while in program mode.
    // Toss the received packets.
    // This is not datatable access, so it's ok.
    for( int i = 0; i < 1000; ++i )
    {
        if( can_port.Recv( &rx_msg ) <= 0 )
            break;
    }

    // Nothing else to do in program or faulted mode.  DT_OBJECTs are
    // off limits in these modes.
}


void TLM::ModeChange( int aNewMode, PLCINT* stsFile )
{
    printf( "%s: SetMode(old=%s, new=%s)\n",
            Name(),
            HlpShowMode( Mode() ),
            HlpShowMode( aNewMode )
            );

    if( aNewMode < OPM_RUN )
    {
        // Zero any real world outputs here.  SoftPLCs turn their
        // outputs off when entering TEST, PROGRAM or FAULT modes.
        // Unless you are directly controlling hardware (not merely setting
        // datatable memory through DT_OBJECTs), then you can assume that the
        // hardware controlling driver will turn off the outputs in this
        // mode change.
    }
}


void TLM::DeInstall()
{
    printf( "%s: deinstalling.\n", Name() );

    can_port.Close();
}