/**
 * @authorSTF 424_ITS_Test_Platform
 * @version    $URL$
 *             $Id$
 */
package org.etsi.its.extfunc;

import java.io.ByteArrayOutputStream;
import java.io.IOException;
import java.math.BigInteger;
import java.util.logging.Logger;

import org.etsi.codec.ITciCDWrapper;
import org.etsi.codec.TciCDWrapperFactory;
import org.etsi.common.ByteHelper;
import org.etsi.ttcn.tci.FloatValue;
import org.etsi.ttcn.tci.IntegerValue;
import org.etsi.ttcn.tci.OctetstringValue;

/**
 * This class implements external ITS function
 * 
 * See TTCN-3 modules LibItsGeoNetworking_Functions and LibItsCommon_Functions
 *
 */
public class ItsExternalFunctionsProvider implements IItsExternalFunctionsProvider {

/**
 * Module version
 */
public static final String Version = "1.0.0.0";

/**
 * Logger instance
 */
private final static Logger _logger = Logger.getLogger("org.etsi.its");

/**
 * Unique instance of TciCDWrapper class
 */
private ITciCDWrapper _tcicdWrapper;

/**
 * Constant factor used for distance computation
 */
private static final double earthRadius = 6378137;
    private static final double rbis = earthRadius * Math.PI / 180;

/**
 * Default ctor
 */
public ItsExternalFunctionsProvider() {
_logger.entering("ItsExternalFunctionsProvider", "Constructor", String.format("version:%s", Version));

_tcicdWrapper = TciCDWrapperFactory.getTciCDInstance();
}

/**
 * This external function gets the current time
 * @return The current time in Epoch format
 * 
 * TTCN-3 signature:
 * external function fx_getCurrentTime() return TimeStamp;
 */
@Override
public synchronized IntegerValue fx_getCurrentTime() {
_logger.entering("ItsExternalFunctionsProvider", "fx_getCurrentTime");

IntegerValue now = _tcicdWrapper.getInteger();
now.setBigInt(BigInteger.valueOf(new Long(System.currentTimeMillis())));

_logger.exiting("ItsExternalFunctionsProvider", "fx_getCurrentTime", now.getBigInt().toString(10));
return now;
} // End of method fx_getCurrentTime

/**
 * This external function gets the current time
 * @param p_latitudeA Latitude of node A
 * @param p_longitudeA Longitude of node A
 * @param p_latitudeB Latitude of node B
 * @param p_longitudeB Longitude of node B
 * @return The current time in Epoch format
 * 
 * TTCN-3 signature:
 * external function fx_computeDistance(in UInt32 p_latitudeA, in UInt32 p_longitudeA, in UInt32 p_latitudeB, in UInt32 p_longitudeB) return float;
 */
@Override
public synchronized FloatValue fx_computeDistance(final IntegerValue p_latitudeA, final IntegerValue p_longitudeA, final IntegerValue p_latitudeB, final IntegerValue p_longitudeB) {
_logger.entering("ItsExternalFunctionsProvider", "fx_computeDistance", String.format("%d, %d, %d, %d", p_latitudeA.getInt(), p_longitudeA.getInt(), p_latitudeB.getInt(), p_longitudeB.getInt()));

// Initialise the returned value
FloatValue dist = _tcicdWrapper.getFloat();
        
        double dlat = (new Double(p_latitudeB.getInt()) - new Double(p_latitudeA.getInt())) / 10000000;
        double dlong = (new Double(p_longitudeB.getInt()) - new Double(p_longitudeA.getInt())) / 10000000;      
        
        long d = Math.round(Math.sqrt(Math.pow(dlat * rbis, 2) + Math.pow(dlong * rbis * Math.cos(dlat), 2)));

        dist.setFloat(d);
        System.out.println("Distance: " + d);

return dist;
} // End of method fx_computeDistance

/**
 * External function to compute a position using a reference position, a distance and an orientation 
 * @param p_iutLongPosVector Reference position
 * @param p_distance Distance to the reference position (in meter)
 * @param p_orientation Direction of the computed position (0 to 359; 0 means North)
 * @param p_latitude Computed position's latitude
 * @param p_longitude Computed position's longitude
 * 
 * TTCN-3 signature:
 * external function fx_computePositionUsingDistance(in LongPosVector p_iutLongPosVector, in integer p_distance, in integer p_orientation, out UInt32 p_latitude, out UInt32 p_longitude);
 */
@Override
public synchronized void fx_computePositionUsingDistance(final IntegerValue p_refLatitude, final IntegerValue p_refLongitude, final IntegerValue p_distance, final IntegerValue p_orientation, IntegerValue p_latitude, IntegerValue p_longitude) {
_logger.entering("ItsExternalFunctionsProvider", "fx_computePositionUsingDistance", String.format("%d, %d", p_distance.getInt(), p_orientation.getInt()));

double angularD = new Double(p_distance.getInt()) / earthRadius;
        double radHeading = new Double(p_orientation.getInt()) * Math.PI / 180;
        
        // Convert to rad
        double lat1 = (new Double(p_refLatitude.getInt()) / 10000000) * Math.PI / 180;
        double long1 = (new Double(p_refLongitude.getInt())  / 10000000) * Math.PI / 180;
        
        double lat2 = Math.asin(Math.sin(lat1) * Math.cos(angularD) + Math.cos(lat1) * Math.sin(angularD) * Math.cos(radHeading));
        double long2 = long1 + Math.atan2(
                Math.sin(radHeading) * Math.sin(angularD) * Math.cos(lat1), 
                Math.cos(angularD) - Math.sin(lat1) * Math.sin(lat2));
        
        // normalise to -180...+180
        long2 = (long2 + 3 * Math.PI) % (2 * Math.PI) - Math.PI;
        
        // convert to 1/10 of microdegrees
        long rlat2 = Math.round(lat2 * 10000000 / Math.PI * 180);
        long rlong2 = Math.round(long2 * 10000000 / Math.PI * 180);

        p_latitude.setInt((int)rlat2);
p_longitude.setInt((int)rlong2);

} // End of method fx_computePositionUsingDistance

/**
     * External function to compute timestamp based on current time
     * @return  Unix-Epoch-Time mod 2^32
     * 
     * TTCN-3 signature:
     * external function fx_computeGnTimestamp() return UInt32;
     */
     public IntegerValue fx_computeGnTimestamp() {
         _logger.entering("ItsExternalFunctionsProvider", "fx_computeGnTimestamp");
         
         IntegerValue timestamp = _tcicdWrapper.getInteger();
         timestamp.setBigInt(BigInteger.valueOf(System.currentTimeMillis() % (long)Math.pow(2,32)));
         
         _logger.exiting("ItsExternalFunctionsProvider", "fx_computeGnTimestamp", timestamp.getBigInt());
         return timestamp;
     }

    /**
     * @desc    Calculate ICMPv6 checksum on pseudo header according RFC 4443 - Clause 2.3
     * @param   p_sourceAddress         Source address (128 bits), 
     * @param   p_destinationAddress    Destination address (128 bits)
     * @param   p_payloadLength         Upper-Layer Packet Length (32 bits)
     * @param	p_payload               Upper-Layer payload
     * @param   p_nextHdr               Next header value (e.g. 0x3a for ICMPv6) (8bits)
     * @return  The checksum value (16bits)
     * @see     RFC 2460 IPv6 Specification
     * 
     * TTCN-3 signature:
     * external function fx_computeIPv6CheckSum( 
     *     in template (value) Ipv6Address p_sourceAddress, 
     *     in template (value) Ipv6Address p_destinationAddress, 
     *     in template (value) integer p_payloadLength, 
     *     in template (value) octetstring p_payload, 
     *     in template (value) integer p_nextHdr 
     * ) return Oct2; 
     * <pre>
     * Pseudo header is defined by RFC 2460 - Clause 8.1
     *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |                                                               |
     *  +                                                               +
     *  |                                                               |
     *  +                         Source Address                        +
     *  |                                                               |
     *  +                                                               +
     *  |                                                               |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |                                                               |
     *  +                                                               +
     *  |                                                               |
     *  +                      Destination Address                      +
     *  |                                                               |
     *  +                                                               +
     *  |                                                               |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |                   Upper-Layer Packet Length                   |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     *  |                      zero                     |  Next Header  |
     *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
     * </pre>
     */
    public OctetstringValue fx_computeIPv6CheckSum(
        final OctetstringValue p_sourceAddress, 
        final OctetstringValue p_destinationAddress, 
        final IntegerValue p_payloadLength, 
        final OctetstringValue p_payload, 
        final IntegerValue p_nextHdr 
    ) {
        _logger.entering("ItsExternalFunctionsProvider", "fx_computeIPv6CheckSum");
        
        // Build the pseudo header according RFC 2460 - Clause 8.1
        ByteArrayOutputStream pseudoheader = new ByteArrayOutputStream();
        // Source address (128bits)
        int i = 0;
        for( ; i < p_sourceAddress.getLength(); i++) {
            pseudoheader.write(p_sourceAddress.getOctet(i));
        } // End of 'for' loop
        // Destination address (128bits)
        for(i = 0; i < p_destinationAddress.getLength(); i++) {
            pseudoheader.write(p_destinationAddress.getOctet(i));
        } // End of 'for' loop
        try {
            // Upper-Layer Packet Length (32bits)
            pseudoheader.write(ByteHelper.intToByteArray(p_payloadLength.getInt(), 4));
            // Checksum set to 0 (24bits)
            pseudoheader.write(ByteHelper.intToByteArray(0, 3));
            // Next header (8bits)
            pseudoheader.write((byte)p_nextHdr.getInt());
        } catch (IOException e) {
            e.printStackTrace();
        }
        // Add the payload
        for(i = 0; i < p_payload.getLength(); i++) {
            pseudoheader.write(p_payload.getOctet(i));
        } // End of 'for' loop
        
        i = 0;
        int length = pseudoheader.size();
        byte[] buffer = pseudoheader.toByteArray();
        ByteHelper.dump("ItsExternalFunctionsProvider.fx_computeIPv6CheckSum: pseaudo header", buffer);
        long sum = 0;
        while (length > 0) {
            sum += (buffer[i++] & 0xff) << 8;
            if ((--length) == 0) { 
                break;
            }
            sum += (buffer[i++] & 0xff);
            --length;
        }
        sum = (~((sum & 0xFFFF)+(sum >> 16)))&0xFFFF;
        _logger.info(String.format("ItsExternalFunctionsProvider.fx_computeIPv6CheckSum: finalSum=%d", sum));
        // Set the return value
        OctetstringValue checksum = _tcicdWrapper.getOctetstring();
        checksum.setLength(2);
        checksum.setOctet(0, (byte)((byte)(sum >> 8) & 0xff));
        checksum.setOctet(1, (byte)(sum & 0x00ff));
        
        _logger.exiting("ItsExternalFunctionsProvider", "fx_computeIPv6CheckSum", checksum); // FIXME Check which method to call for logging
        return checksum;
    }
    
} // End of class ExternalFunctions
