/*
 * Testbeispiel: Pfadsuche
 */

#include "sw_config.h"



#if defined( INCLUDE_UNIT_TESTS ) || defined( DOXYGEN )

    #include "eg_main_control_private.h"
    #include "unity_fixture.h"
    #include <stdio.h>
    #include <string.h>



    /**Zweier-Logarithmus (32 Bit) */
    static_inline uint8_t log2_u32(uint32_t a)
    {
        if( a == 0 )
            return 0;
        else
            return 31-__builtin_clzl(a);
    }



    /** Adjazenzmatrix aus Knotenliste erstellen
     *
     * @param[out] Adj: Adjazenzmatrix, Array-Index Quellknoten, Bit-Position ist Zielknoten
     * @param[in] nNode: Anzahl der Knoten
     * @param[in] vertices[]: Kantenliste (gerichtet)
     * @param[in] nVert: Laenge der Kantenliste */
    void adjacenceMatrix(uint32_t *Adj, uint8_t nNode, uint8_t vertices[][2], uint8_t nVertices)
    {
        assert( Adj != NULL );

        /* Adjazenzmatrix ist 32 Bit breit, das begrenzt die Anzahl der erlaubten Zustaende */
        assert(nNode <= 32);

        /* Nullen */
        for( int i=0; i<nNode; i++ )
        {
            *(Adj+i) = 0;
        }

        /* Abgedeckte Knoten markieren */
        for( int i=0; i<nVertices; i++ )
        {
            uint_fast8_t src = vertices[i][0];
            uint_fast8_t dst = vertices[i][1];

            if( (src > nNode) || (dst > nNode ) )
            {
                error("Index exceeds matrix dimensions.");
            }
            *(Adj+src) |= 1U<<dst;
        }
    }



    /** Pfad in einem gerichteten Graphen finden
     * Graph darf maximal 32 Knoten enthalten.
     *
     * Implementiert nach https://de.wikipedia.org/wiki/Breitensuche + eigene Ergänzung für
     * Knotenliste
     *
     * @param[in] Adj: Adjazenzmatrix
     * @param[in] nNode: Anzahl der Knoten
     * @param[in] start, dest: Startknoten, Zielknoten
     * @param[out] path: Pfad rueckwaerts vom Ziel zum Start, -1 als Endmarker
                        muss Platz für nNode Elemente enthalten.
     * @return Nächster Knoten, -1 wenn nicht gefunden */
    uint8_t findPath(uint32_t Adj[], uint8_t nNode, uint8_t start, uint8_t dest, uint8_t *path)
    {
        uint8_t next = -1;                          /* Rückgabewert */

        assert( Adj != NULL );
        assert(nNode <= 32);


        /* Initialisierung */
        bool found = false;
        uint32_t visited = 1U<<start;               /* Besuchte Knoten */
        uint32_t queued = 1U<<start;                /* Warteschlange (unsortiert) */
        uint8_t parent[nNode];                      /* Elternknoten */


        /* Breitensuche */
        while( queued )
        {
            /* Erstes Element aus Warteschlange entnehmen, von links, da ARM _CLZ() hat */
            uint8_t iNode = log2_u32(queued);       /* Knotenindex */
            queued &= ~(1U<<iNode);

            if( iNode == dest )
            {
                found = true;
                break;
            }

            uint32_t children = Adj[iNode];         /* Kindelemente (Bitmaske) */
            children &= ~visited;
            queued |= children;
            visited |= children;
            while( children )
            {
                uint8_t child = log2_u32(children); /* Erstes Kindelement (Index) */
                children &= ~(1U<<child);
                parent[child]= iNode;
            }
        }


        if( found )
        {
            if( path != NULL )
            {
                /* Kompletten Pfad rueckwarts rekonstruieren (wenn gefragt) */
                memset(path, -1, nNode);
                path[0] = dest;
                int i = 0;
                while( path[i] != start )
                {
                    uint8_t p = parent[path[i]];
                    path[++i] = p;
                }
            }

            /* Naechsten Knoten rueckwarts rekonstruieren (Rueckgabewert) */
            next = dest;
            if( next != start )
            {
                while( parent[next] != start )
                {
                    next = parent[next];
                }
            }
        }

        return next;
    }



    /** Lookup-Table für alle erlaubten und nicht-erlaubten Zustandsänderungen
     * (von jedem zu jedem Knoten *)
     *
     * @param[out] lut: Lookup-Table, lut[nNode][nNode]
     * @param[in] Adj:  Adjazenz-Matrix
     * @param[in] nNode: Anzahl der Knoten */
    void lut(void *lut, uint32_t Adj[], uint8_t nNode)
    {
        uint8_t (*alut)[nNode][nNode] = (uint8_t (*)[nNode][nNode]) lut;

        for( int src=0; src<nNode; src++ )
        {
            for( int dst=0; dst<nNode; dst++ )
            {
                (*alut)[src][dst] = findPath(Adj, nNode, src, dst, NULL);
            }
        }
    }



    TEST_GROUP(statemachine);
    TEST_SETUP(statemachine) {}
    TEST_TEAR_DOWN(statemachine) {}


    TEST_GROUP_RUNNER(statemachine)
    {
        RUN_TEST_CASE(statemachine, adjacenceMatrix);
        RUN_TEST_CASE(statemachine, findPath);
        RUN_TEST_CASE(statemachine, lut);
    }



	/* Liste Zustände */
	enum tst_state_e
	{
		pre_init,
		init,
		sync,
		manual,
		forward,
		reverse,
		de_init,
		disabled,
		endmarker_,
	};


	/* Erlaubte Zustandsänderungen als (gerichtete) Kanten eines Graphen */
	uint8_t tst_transitions[][2] =
	{
		{ pre_init, init },
		{ init,     sync },
		{ sync,     manual },
		{ manual,   sync },
		{ sync,     forward },
		{ forward,  sync },
		{ sync,     reverse },
		{ reverse,  sync },
		{ sync,     de_init },
		{ de_init,  disabled },
	};


    TEST(statemachine, adjacenceMatrix)
    {
		uint32_t tst_adj[endmarker_];
		
        int nVert = sizeof(tst_transitions)/sizeof(tst_transitions[0]);
        int nNode = endmarker_;
        adjacenceMatrix(tst_adj, nNode, transitions, nVert);
        printf("\n");
        for(int i = 0; i<nNode; i++)
        {

            printf("%2i: 0x%04x\n", i, tst_adj[i]);
        }

        prepareErrorCondition();
        if( TEST_PROTECT() )
        {
            adjacenceMatrix(NULL, nNode, transitions, nVert);
        }
        TEST_ASSERT_TRUE( isErrorConditionTriggered() );
        clearErrorCondition();
    }



    TEST(statemachine, findPath)
    {
		uint32_t tst_adj[endmarker_];
		
        int nVert = sizeof(transitions)/sizeof(transitions[0]);
        int nNode = ctrl_state_endmarker_;
        uint32_t *Adj = tst_adj;
        adjacenceMatrix(Adj, nNode, transitions, nVert);


        TEST_ASSERT_EQUAL( 18, nVert );
        TEST_ASSERT_EQUAL( forward, findPath(Adj, nNode, sync, forward, NULL) );
        TEST_ASSERT_EQUAL( forward, findPath(Adj, nNode, forward, forward, NULL) );
        TEST_ASSERT_EQUAL( sync, findPath(Adj, nNode, reverse, forward, NULL) );
        TEST_ASSERT_EQUAL( init, findPath(Adj, nNode, pre_init, forward, NULL) );

        prepareErrorCondition();
        if( TEST_PROTECT() )
        {
            findPath(NULL, nNode, pre_init, forwrd, NULL);
        }
        TEST_ASSERT_TRUE( isErrorConditionTriggered() );
        clearErrorCondition();
    }



    TEST(statemachine, lut)
    {
        uint32_t tst_adj[endmarker_];
        uint8_t tst_LUT[endmarker_][endmarker_];

        int nNode = endmarker_;

        /* Adjazenzmatrix vorbereiten */
        {
            int nVert = sizeof(tst_transitions)/sizeof(tst_transitions[0]);
            adjacenceMatrix(tst_adj, nNode, tst_transitions, nVert);
        }

        /* LUT erstellen */
        lut(tst_LUT, tst_adj, nNode);


        /* Darstellung auf der Konsole */
        printf("\nLUT[] = {");
        for( int i=0; i<nNode; i++ )
        {
            printf("\n    ");
            for( int j=0; j<nNode; j++ )
            {
                int8_t t = tst_LUT[i][j];
                if( t == -1 )
                    printf(" *, ");
                else
                    printf("%2i, ", t);
            }
        }
        printf("\n}\n");
    }



#else
    typedef int this_is_no_empty_translation_unit;

#endif