/**
 *  GeoNetworking port implementation (background thread)
 *  
 *  @author     ETSI / STF424
 *  @version    $URL$
 *              $Id$
 *
 */
package org.etsi.its.adapter.ports;

import java.util.HashMap;
import java.util.Map;

import org.etsi.adapter.TERFactory;
import org.etsi.common.ByteHelper;
import org.etsi.common.KPM;
import org.etsi.its.adapter.layers.EthernetLayer;
import org.etsi.its.adapter.layers.IEthernetSpecific;
import org.etsi.ttcn.tci.CharstringValue;

import de.fraunhofer.sit.c2x.CryptoLib;

/**
 *  GeoNetworking port implementation (background thread)
 */
public class GnPort extends ProtocolPort implements Runnable, IEthernetSpecific {

    /**
     * GeoNetworking header type for beacon messages
     */
    private static final int HT_BEACON = 1;
    private static final int HT_TSB = 5;
    private static final int HST_SHB = 0;
    
    
    /**
     * Constructor
     * @param   portName        Name of the port
     * @param   componentName   Name of the component owning this port instance
     * @param   lowerStackDesc  Description of the port's lower stack in the form "Layer/Layer/Layer/..."
     * @param  linkLayerAddress    Link-layer address to be used by this port as source address (null if not applicable)
     */
    public GnPort(String portName, String componentName, String lowerStackDesc, String linkLayerAddress) {
        super(portName, componentName, lowerStackDesc, linkLayerAddress);
        running = true;
        management.registerGnPort(this);
    }

    /* (non-Javadoc)
     * @see org.etsi.its.adapter.ports.IEthernetSpecific#getEthernetType()
     */
    @Override
    public short getEthernetType() {
        
        // Retrieve EthernetType value
        Integer iutEthernetTypeValue = Integer.decode(((CharstringValue)TERFactory.getInstance().getTaParameter("IutEthernetTypeValue")).getString());
        return iutEthernetTypeValue.shortValue();
    }

    /**
     * Starts beaconning
     */
    public void startBeaconning() {
        running = true;
        if(beaconThread == null) {
            beaconThread = new Thread(this);
            beaconThread.start();
        }
    }
    
    /**
     * Stops beaconning
     */
    public void stopBeaconning() {
        if(beaconThread != null) {
            running = false;
            try {
                beaconThread.join();
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
            beaconThread = null;
        }
    }
    
    /**
     * Thread function for sending periodic beacons
     * @see java.lang.Runnable#run()
     */
    @Override
    public void run() {
        byte[] beaconHeader = management.getGnBeacon();
        if(beaconHeader != null) {
            long modulo = (long)Math.pow(2,32);
            Map<String, Object> params = new HashMap<String, Object>();
            long triggerTime = System.currentTimeMillis();
            while(running) {
                // Update timestamp. Timestamp is 1s older than current time to avoid sending beacons coming from the future (time sync between nodes)
                byte[] tst = ByteHelper.intToByteArray((int)(((System.currentTimeMillis() - 1072915200000L) % modulo) - 3000), 4);
                System.arraycopy(tst, 0, beaconHeader, 20, 4);
                if (!management.isSecuredModeSet()) { // Secure mode disabled
                    send(beaconHeader, params);
                } else { // Send a secured beacon
                    // Retrieve the secured data template
                    byte[] toBeSignedData;
                    if ((System.currentTimeMillis() - triggerTime) < 1000) {
                        toBeSignedData = management.getToBeSignedDataDigest(); 
                    } else {
                        triggerTime = System.currentTimeMillis();
                        toBeSignedData = management.getToBeSignedDataCertificate(); 
                    }
                    // Set marker: generation_time field.
                    int timestamps = KPM.indexOf(toBeSignedData, 0, toBeSignedData.length - 1, new byte[] { (byte)0xbb, (byte)0xbb, (byte)0xbb, (byte)0xbb } );
                    // Set marker: Index of the length of payload field
                    int payloadIndex = KPM.indexOf(toBeSignedData, 0, toBeSignedData.length - 1, new byte[] { (byte)0xaa, (byte)0xaa, (byte)0xaa, (byte)0xaa, (byte)0xaa } ) - 1;
                    // Replace current time & payload
                    toBeSignedData = ByteHelper.concat(
                        ByteHelper.extract(toBeSignedData, 0, timestamps),
                        tst,
                        ByteHelper.extract(toBeSignedData, timestamps + tst.length, payloadIndex - timestamps - tst.length),
                        new byte[] { (byte)beaconHeader.length },
                        beaconHeader,
                        ByteHelper.extract(toBeSignedData, payloadIndex + toBeSignedData[payloadIndex] + 1, toBeSignedData.length - 1 - (payloadIndex + toBeSignedData[payloadIndex]))
                    );
//                    System.out.println("GnPort: toBeSignedData " + ByteHelper.byteArrayToString(toBeSignedData));
                    // Calculate the hash
                    byte[] hash = CryptoLib.hashWithSha256(toBeSignedData);
                    try {
                        // Signed the hash
                        byte[] signatureBytes = CryptoLib.signWithEcdsaNistp256WithSha256(hash, management.getSigningPrivateKey());
//                        System.out.println("GnPort: signatureBytes " + ByteHelper.byteArrayToString(signatureBytes));
                        // Add signature
                        byte[] securedBeaconHeader = ByteHelper.concat(
                            new byte[] { (byte)0x02, (byte)0x00, (byte)0x2B, (byte)0x0A }, // GeoNet Basic header
                            toBeSignedData,
                            new byte[] { 0x01, 0x00, 0x02 }, // Signature header
                            ByteHelper.extract(
                                signatureBytes,
                                2,
                                signatureBytes.length - 2
                        ));
                        // Send the secured beacon
//                        System.out.println("GnPort: Call send " + ByteHelper.byteArrayToString(securedBeaconHeader));
                        send(securedBeaconHeader, params);
                    } catch (Exception e) {
                        // Do not send the beacon
                        e.printStackTrace();
                    }
                }
                try {
                    Thread.sleep(management.getGnBeaconInterval());
                } catch (InterruptedException e) {
                    // Do nothing, we do not care
                }
            }
        }
    }

    /* (non-Javadoc)
     * @see org.etsi.its.adapter.ports.IPort#send(byte[])
     */
    @Override
    public boolean send(byte[] message) {
        HashMap<String, Object> params = new HashMap<String, Object>();
        
        byte[] destMacAddress = ByteHelper.extract(message, message.length - 6, 6);
        message = ByteHelper.extract(message, 0, message.length - 6);
        params.put(EthernetLayer.LINK_LAYER_DESTINATION, destMacAddress);
        ByteHelper.dump("GnPort.send", message);
        return send(message, params);
    }
    
    /* (non-Javadoc)
     * @see org.etsi.its.adapter.ports.ProtocolPort#receive(byte[])
     */
    @Override
    public void receive(byte[] message, Map<String, Object> lowerInfo) {
//        System.out.println(">>> GnPort.receive: " + ByteHelper.byteArrayToString(message));
        
        byte[] basicHdr = new byte[4];
        System.arraycopy(message, 0, basicHdr, 0, 4);
        int headerType = -1;
        int headerSubType = -1;
        byte[] sopv = new byte[24];
        if (!management.isSecuredModeSet()) { // Secure mode disabled
            byte[] commonHdr = new byte[8];
            System.arraycopy(message, 4, commonHdr, 0, 8);
            byte[] htHst = new byte[1];
            System.arraycopy(commonHdr, 1, htHst, 0, 1);
            headerType = (int)(htHst[0] >> 4);
            headerSubType = (int)(htHst[0] & 0x000000000F);
            // Update LPV table
            int sopvPos = 12;
            if(headerType != HT_BEACON && !(headerType == HT_TSB && headerSubType == HST_SHB) ) {
                sopvPos += 4;
            }        
            System.arraycopy(message, sopvPos, sopv, 0, 24);
        } else {
            if ((basicHdr[0] & 0x0f) == 0x02) { // Secured tag
                int offset = basicHdr.length + 2;
                int signerInfoTypeIndex = 0;
                // Extract Header length (IntX type)
                if ((message[offset] & 0x80) == 0x80) { // Integer >= 128
                    byte msb = message[offset]; // TODO Create a method decodeIntX
                    byte bit;
                    byte byteLen = 1;
                    do {
                        bit = (byte) ((byte) (msb << byteLen++) & 0x80);
                    } while (bit != 0x00);
                    // Set the IntX length
                    byte[] newBuf = ByteHelper.extract(message, offset, byteLen);
                    // Remove the length from the real integer value
                    newBuf[0] &= (byte)(Math.pow(2.0, 8 - byteLen + 1) - 1);
                    signerInfoTypeIndex = offset + byteLen;
                    offset += byteLen + ByteHelper.byteArrayToInt(newBuf);
                } else {
                    signerInfoTypeIndex = offset + 1;
                    offset += message[offset] + 1;
                }
                // Extract Payload length (IntX type)
                if ((message[offset] & 0x80) == 0x80) { // Integer >= 128
                    byte msb = message[offset];
                    byte bit;
                    byte byteLen = 1;
                    do {
                        bit = (byte) ((byte) (msb << byteLen++) & 0x80);
                    } while (bit != 0x00);
                    offset += byteLen;
                } else {
                    offset += 1;
                }
                offset += 1; // Skip Payload Type
                // Extract Data Payload length (IntX type)
                if ((message[offset] & 0x80) == 0x80) { // Integer >= 128
                    byte msb = message[offset];
                    byte bit;
                    byte byteLen = 1;
                    do {
                        bit = (byte) ((byte) (msb << byteLen++) & 0x80);
                    } while (bit != 0x00);
                    offset += byteLen;
                } else {
                    offset +=1;
                }
                // Sanity check: Verify SignerInfo field
                if (
                    ((message[signerInfoTypeIndex] & 0x80) != 0x80) ||  // SignerInfo Type: certificate digest with ecdsap256 (1)
                    (
                        (message[signerInfoTypeIndex + 1] != 0x01) &&   // SignerInfo Type: certificate digest with ecdsap256 (1)
                        (message[signerInfoTypeIndex + 1] != 0x02)      // SignerInfo Type: certificate (2)
                    )
                ) {
                    // Drop it
                    return;
                }
                byte[] commonHdr = new byte[8];
                System.arraycopy(message, offset, commonHdr, 0, 8);
                byte[] htHst = new byte[1];
                System.arraycopy(commonHdr, 1, htHst, 0, 1);
                headerType = (int)(htHst[0] >> 4);
                headerSubType = (int)(htHst[0] & 0x000000000F);
                // Update LPV table
                int sopvPos = offset + commonHdr.length;
                if(headerType != HT_BEACON && !(headerType == HT_TSB && headerSubType == HST_SHB) ) {
                    sopvPos += 4;
                }        
                System.arraycopy(message, sopvPos, sopv, 0, 24);
            } else {
                // Drop it
                return;
            }
        }
        
        byte[] gn = new byte[8];
        System.arraycopy(sopv, 0, gn, 0, 8);
        byte[] mid = new byte[6];
        System.arraycopy(gn, 2, mid, 0, 6);
        byte[] tst = new byte[4];
        System.arraycopy(sopv, 8, tst, 0, 4);
        management.gnUpdateLocTable(mid, ByteHelper.byteArrayToInt(tst), sopv);
        
        // Filter beacons
        byte [] beaconFilter = management.getGnEnqueueBeacon();
        if(headerType != HT_BEACON || beaconFilter != null) {
            if(headerType == HT_BEACON) {
                byte[] filterMid = new byte[6];
                System.arraycopy(beaconFilter, 2, filterMid, 0, 6);
                if(java.util.Arrays.equals(mid, filterMid) == false) {
                    // Received beacon does not match filter
                    return;
                }
            }    
            // Encode with GN indication header
            // Extract LINK_LAYER_DESTINATION
            byte[] msgInd = ByteHelper.concat(message, (byte[])lowerInfo.get(EthernetLayer.LINK_LAYER_DESTINATION));
            super.receive(msgInd, lowerInfo);
        }
    }

    /* (non-Javadoc)
     * @see org.etsi.its.adapter.ports.ProtocolPort#dispose()
     */
    @Override
    public void dispose() {
        if(running && beaconThread != null) {
            running = false;
            try {
                beaconThread.join();
            } catch (InterruptedException e) {
                e.printStackTrace();
            }
        }
        super.dispose();
    }
    
    /**
     * Indicates whether the port is still active. Setting this field to false will cause
     * the beaconing thread to stop its execution.
     */
    private volatile boolean running;
    
    /**
     * Beaconing thread instance.
     */
    private Thread beaconThread = null;
}

