// RHMesh.cpp
//
// Define addressed datagram
//
// Part of the Arduino RH library for operating with HopeRF RH compatible transceivers
// (see http://www.hoperf.com)
// RHDatagram will be received only by the addressed node or all nodes within range if the
// to address is RH_BROADCAST_ADDRESS
//
// Author: Mike McCauley (mikem@airspayce.com)
// Copyright (C) 2011 Mike McCauley
// $Id: RHMesh.cpp,v 1.9 2015/08/13 02:45:47 mikem Exp $

#include <RHMesh.h>

uint8_t RHMesh::_tmpMessage[RH_ROUTER_MAX_MESSAGE_LEN];

////////////////////////////////////////////////////////////////////
// Constructors
RHMesh::RHMesh(RHGenericDriver& driver, uint8_t thisAddress)
    : RHRouter(driver, thisAddress) {
}

////////////////////////////////////////////////////////////////////
// Public methods

////////////////////////////////////////////////////////////////////
// Discovers a route to the destination (if necessary), sends and
// waits for delivery to the next hop (but not for delivery to the final destination)
uint8_t RHMesh::sendtoWait(uint8_t* buf, uint8_t len, uint8_t address, uint8_t flags) {
    if (len > RH_MESH_MAX_MESSAGE_LEN) {
        return RH_ROUTER_ERROR_INVALID_LENGTH;
    }

    if (address != RH_BROADCAST_ADDRESS) {
        RoutingTableEntry* route = getRouteTo(address);
        if (!route && !doArp(address)) {
            return RH_ROUTER_ERROR_NO_ROUTE;
        }
    }

    // Now have a route. Contruct an application layer message and send it via that route
    MeshApplicationMessage* a = (MeshApplicationMessage*)&_tmpMessage;
    a->header.msgType = RH_MESH_MESSAGE_TYPE_APPLICATION;
    memcpy(a->data, buf, len);
    return RHRouter::sendtoWait(_tmpMessage, sizeof(RHMesh::MeshMessageHeader) + len, address, flags);
}

////////////////////////////////////////////////////////////////////
bool RHMesh::doArp(uint8_t address) {
    // Need to discover a route
    // Broadcast a route discovery message with nothing in it
    MeshRouteDiscoveryMessage* p = (MeshRouteDiscoveryMessage*)&_tmpMessage;
    p->header.msgType = RH_MESH_MESSAGE_TYPE_ROUTE_DISCOVERY_REQUEST;
    p->destlen = 1;
    p->dest = address; // Who we are looking for
    uint8_t error = RHRouter::sendtoWait((uint8_t*)p, sizeof(RHMesh::MeshMessageHeader) + 2, RH_BROADCAST_ADDRESS);
    if (error !=  RH_ROUTER_ERROR_NONE) {
        return false;
    }

    // Wait for a reply, which will be unicast back to us
    // It will contain the complete route to the destination
    uint8_t messageLen = sizeof(_tmpMessage);
    // FIXME: timeout should be configurable
    unsigned long starttime = millis();
    int32_t timeLeft;
    while ((timeLeft = RH_MESH_ARP_TIMEOUT - (millis() - starttime)) > 0) {
        if (waitAvailableTimeout(timeLeft)) {
            if (RHRouter::recvfromAck(_tmpMessage, &messageLen)) {
                if (messageLen > 1
                        && p->header.msgType == RH_MESH_MESSAGE_TYPE_ROUTE_DISCOVERY_RESPONSE) {
                    // Got a reply, now add the next hop to the dest to the routing table
                    // The first hop taken is the first octet
                    addRouteTo(address, headerFrom());
                    return true;
                }
            }
        }
        YIELD;
    }
    return false;
}

////////////////////////////////////////////////////////////////////
// Called by RHRouter::recvfromAck whenever a message goes past
void RHMesh::peekAtMessage(RoutedMessage* message, uint8_t messageLen) {
    MeshMessageHeader* m = (MeshMessageHeader*)message->data;
    if (messageLen > 1
            && m->msgType == RH_MESH_MESSAGE_TYPE_ROUTE_DISCOVERY_RESPONSE) {
        // This is a unicast RH_MESH_MESSAGE_TYPE_ROUTE_DISCOVERY_RESPONSE messages
        // being routed back to the originator here. Want to scrape some routing data out of the response
        // We can find the routes to all the nodes between here and the responding node
        MeshRouteDiscoveryMessage* d = (MeshRouteDiscoveryMessage*)message->data;
        addRouteTo(d->dest, headerFrom());
        uint8_t numRoutes = messageLen - sizeof(RoutedMessageHeader) - sizeof(MeshMessageHeader) - 2;
        uint8_t i;
        // Find us in the list of nodes that were traversed to get to the responding node
        for (i = 0; i < numRoutes; i++)
            if (d->route[i] == _thisAddress) {
                break;
            }
        i++;
        while (i++ < numRoutes) {
            addRouteTo(d->route[i], headerFrom());
        }
    } else if (messageLen > 1
               && m->msgType == RH_MESH_MESSAGE_TYPE_ROUTE_FAILURE) {
        MeshRouteFailureMessage* d = (MeshRouteFailureMessage*)message->data;
        deleteRouteTo(d->dest);
    }
}

////////////////////////////////////////////////////////////////////
// This is called when a message is to be delivered to the next hop
uint8_t RHMesh::route(RoutedMessage* message, uint8_t messageLen) {
    uint8_t from = headerFrom(); // Might get clobbered during call to superclass route()
    uint8_t ret = RHRouter::route(message, messageLen);
    if (ret == RH_ROUTER_ERROR_NO_ROUTE
            || ret == RH_ROUTER_ERROR_UNABLE_TO_DELIVER) {
        // Cant deliver to the next hop. Delete the route
        deleteRouteTo(message->header.dest);
        if (message->header.source != _thisAddress) {
            // This is being proxied, so tell the originator about it
            MeshRouteFailureMessage* p = (MeshRouteFailureMessage*)&_tmpMessage;
            p->header.msgType = RH_MESH_MESSAGE_TYPE_ROUTE_FAILURE;
            p->dest = message->header.dest; // Who you were trying to deliver to
            // Make sure there is a route back towards whoever sent the original message
            addRouteTo(message->header.source, from);
            ret = RHRouter::sendtoWait((uint8_t*)p, sizeof(RHMesh::MeshMessageHeader) + 1, message->header.source);
        }
    }
    return ret;
}

////////////////////////////////////////////////////////////////////
// Subclasses may want to override
bool RHMesh::isPhysicalAddress(uint8_t* address, uint8_t addresslen) {
    // Can only handle physical addresses 1 octet long, which is the physical node address
    return addresslen == 1 && address[0] == _thisAddress;
}

////////////////////////////////////////////////////////////////////
bool RHMesh::recvfromAck(uint8_t* buf, uint8_t* len, uint8_t* source, uint8_t* dest, uint8_t* id, uint8_t* flags) {
    uint8_t tmpMessageLen = sizeof(_tmpMessage);
    uint8_t _source;
    uint8_t _dest;
    uint8_t _id;
    uint8_t _flags;
    if (RHRouter::recvfromAck(_tmpMessage, &tmpMessageLen, &_source, &_dest, &_id, &_flags)) {
        MeshMessageHeader* p = (MeshMessageHeader*)&_tmpMessage;

        if (tmpMessageLen >= 1
                && p->msgType == RH_MESH_MESSAGE_TYPE_APPLICATION) {
            MeshApplicationMessage* a = (MeshApplicationMessage*)p;
            // Handle application layer messages, presumably for our caller
            if (source) {
                *source = _source;
            }
            if (dest) {
                *dest   = _dest;
            }
            if (id) {
                *id     = _id;
            }
            if (flags) {
                *flags  = _flags;
            }
            uint8_t msgLen = tmpMessageLen - sizeof(MeshMessageHeader);
            if (*len > msgLen) {
                *len = msgLen;
            }
            memcpy(buf, a->data, *len);

            return true;
        } else if (_dest == RH_BROADCAST_ADDRESS
                   && tmpMessageLen > 1
                   && p->msgType == RH_MESH_MESSAGE_TYPE_ROUTE_DISCOVERY_REQUEST) {
            MeshRouteDiscoveryMessage* d = (MeshRouteDiscoveryMessage*)p;
            // Handle Route discovery requests
            // Message is an array of node addresses the route request has already passed through
            // If it originally came from us, ignore it
            if (_source == _thisAddress) {
                return false;
            }

            uint8_t numRoutes = tmpMessageLen - sizeof(MeshMessageHeader) - 2;
            uint8_t i;
            // Are we already mentioned?
            for (i = 0; i < numRoutes; i++)
                if (d->route[i] == _thisAddress) {
                    return false;    // Already been through us. Discard
                }

            // Hasnt been past us yet, record routes back to the earlier nodes
            addRouteTo(_source, headerFrom()); // The originator
            for (i = 0; i < numRoutes; i++) {
                addRouteTo(d->route[i], headerFrom());
            }
            if (isPhysicalAddress(&d->dest, d->destlen)) {
                // This route discovery is for us. Unicast the whole route back to the originator
                // as a RH_MESH_MESSAGE_TYPE_ROUTE_DISCOVERY_RESPONSE
                // We are certain to have a route there, because we just got it
                d->header.msgType = RH_MESH_MESSAGE_TYPE_ROUTE_DISCOVERY_RESPONSE;
                RHRouter::sendtoWait((uint8_t*)d, tmpMessageLen, _source);
            } else if (i < _max_hops) {
                // Its for someone else, rebroadcast it, after adding ourselves to the list
                d->route[numRoutes] = _thisAddress;
                tmpMessageLen++;
                // Have to impersonate the source
                // REVISIT: if this fails what can we do?
                RHRouter::sendtoFromSourceWait(_tmpMessage, tmpMessageLen, RH_BROADCAST_ADDRESS, _source);
            }
        }
    }
    return false;
}

////////////////////////////////////////////////////////////////////
bool RHMesh::recvfromAckTimeout(uint8_t* buf, uint8_t* len, uint16_t timeout, uint8_t* from, uint8_t* to, uint8_t* id,
                                uint8_t* flags) {
    unsigned long starttime = millis();
    int32_t timeLeft;
    while ((timeLeft = timeout - (millis() - starttime)) > 0) {
        if (waitAvailableTimeout(timeLeft)) {
            if (recvfromAck(buf, len, from, to, id, flags)) {
                return true;
            }
            YIELD;
        }
    }
    return false;
}



