/**
 * @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.net.InetAddress;
import java.net.UnknownHostException;
import java.text.DateFormat;
import java.text.ParseException;
import java.text.SimpleDateFormat;
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.CharstringValue;
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;

    private static final long ITS_REF_TIME = 1072915200000L;

    /**
     * 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
     *         TimestampIts;
     */
    @Override
    public synchronized IntegerValue fx_getCurrentTime() {
        _logger.entering("ItsExternalFunctionsProvider", "fx_getCurrentTime");
        
        String datestr="01/01/2004 00:00:00";
        DateFormat formatter = new SimpleDateFormat("MM/dd/yyyy hh:mm:ss");
        IntegerValue now = _tcicdWrapper.getInteger();
        try {
//            System.out.println(String.format("fx_getCurrentTime: now:%d, 2004:%d", System.currentTimeMillis(), ((java.util.Date)formatter.parse(datestr)).getTime()));
//            System.out.println("Diff: " + (int)(System.currentTimeMillis() - ((java.util.Date)formatter.parse(datestr)).getTime()));
            now.setBigInt(
                new BigInteger(
                    1, 
                    ByteHelper.longToByteArray(
                        System.currentTimeMillis() - ((java.util.Date)formatter.parse(datestr)).getTime(),
                        Long.SIZE / Byte.SIZE
                    )
                )
            );
        } catch (ParseException e) {
            now.setInt(0);
        }
        
        _logger.exiting("ItsExternalFunctionsProvider", "fx_getCurrentTime", String.format("%10d", now.getInt())); 
        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

    @Override
    public FloatValue fx_computeRadiusFromCircularArea(FloatValue p_squareMeters) {

        // Initialise the returned value
        FloatValue radius = _tcicdWrapper.getFloat();
        
        radius.setFloat((float) Math.sqrt(p_squareMeters.getFloat() / Math.PI));
        
        return radius;
    }
    
    /**
     * 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;
     */
    @Override
    public IntegerValue fx_computeGnTimestamp() {

        IntegerValue timestamp = _tcicdWrapper.getInteger();
        
        // /!\ ttwb specific
        timestamp.setBigInt(new BigInteger(1, ByteHelper.longToByteArray(((System.currentTimeMillis() - ITS_REF_TIME) % (long)Math.pow(2,32)), 5)));
        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>
     */
    @Override
    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;
    }

    @Override
    public OctetstringValue xf_parseIpv6Address(
            CharstringValue p_textIpv6Address) {

        byte[] hexIpv6Address = null;

        try {
            InetAddress ipv6Address = InetAddress.getByName(p_textIpv6Address
                    .getString());
            hexIpv6Address = ipv6Address.getAddress();
        } catch (UnknownHostException e) {
            e.printStackTrace();
        }

        OctetstringValue result = _tcicdWrapper.getOctetstring();
        result.setLength(hexIpv6Address.length);
        for (int i = 0; i < hexIpv6Address.length; i++) {
            result.setOctet(i, hexIpv6Address[i]);
        }
        return result;
    }

} // End of class ExternalFunctions