using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace ForTestingStuff
{
    class Program
    {
        public static Int64 lat_r = 407890123 * 2 + 407890123;

        static void Main(string[] args)
        {
            Byte[] arr = SendGPS();

            double lat = ProcessLat(arr, 20, 4);

            Console.WriteLine("lat:"+lat);

            double lng = ProcessLng(arr, 24, 4);

            Console.WriteLine("lng:"+ lng);

            Byte[] arr2 = Invers_Lat(lat);
            Console.WriteLine(arr2);
            Console.ReadKey();
        }
        static Byte[] Invers_Lat(Double l)
        {
            Byte[] ret = new Byte[4];
            Double lat = l;
            bool sign = false;
            if (lat < 0)
            {
                sign = true;
                lat *= -1;
            }

            lat *= 2;
            lat *= 1024;
            lat *= 1024;
            lat *= 1024;
            lat /= 90;

            ulong ul = (ulong)lat;
            ret = BitConverter.GetBytes(ul);
            if (sign)
                ret[3] |= 0x80;
            return ret;
        }

        static Byte[] Invers_LNG(Double l)
        {
            Byte[] ret = new Byte[4];
            Double lng = l;
            bool sign = false;
            if (lng < 0)
            {
                sign = true;
                lng *= -1;
                lng = 180 - lng;
            }

            lng *= 4;
            lng *= 1024;
            lng *= 1024;
            lng *= 1024;
            lng /= 360;

            ulong ul = (ulong)lng;
            ret = BitConverter.GetBytes(ul);
            if (sign)
                ret[3] |= 0x80;
            return ret;
        }

        static double ProcessLat(Byte[] data, int startIndex, int len)
        {
            bool sign = false;
            if ((data[startIndex] & 0x80) != 0)
            {
                sign = true;
            }
            ulong l = (ulong)data[startIndex];
            if (sign)
                l ^= 0x80; // remove the sign

            l <<= 8;
            l |= (ulong)data[startIndex + 1];
            l <<= 8;
            l |= (ulong)data[startIndex + 2];
            l <<= 8;
            l |= (ulong)data[startIndex + 3];


            //Console.WriteLine("lat ulong=0x" + l.ToString("X"));

            double ld = (double)l;
            ld *= 90;
            ld /= 1024;
            ld /= 1024;
            ld /= 1024;
            ld /= 2;
            if (sign)
                ld *= -1;

            return ld;
        }

        static double ProcessLng(Byte[] data, int startIndex, int len)
        {
            bool sign = false;
            if ((data[startIndex] & 0x80) != 0)
            {
                sign = true;
            }
            //Console.WriteLine("data[]=" + data[startIndex].ToString("X") + " sign=" + sign);
            ulong l = (ulong)data[startIndex];
            if (sign)
                l ^= 0x80; // remove the sign

            l <<= 8;
            l |= (ulong)data[startIndex + 1];
            l <<= 8;
            l |= (ulong)data[startIndex + 2];
            l <<= 8;
            l |= (ulong)data[startIndex + 3];


            //Console.WriteLine("lng ulong=0x" + l.ToString("X"));

            double ld = (double)l;
            ld *= 360;
            ld /= 1024;
            ld /= 1024;
            ld /= 1024;
            ld /= 4;

            if (sign)
            {
                ld = 180 - ld;
                ld *= -1;
            }
            return ld;
        }

        private static Byte[] SendGPS()
        {            
            //Console.WriteLine("Send Mess ON UDP unit:"+unitID+"unit State:"+unitState);
            Byte[] sendBytes = { 0x0D, 0x1F, 0x22, 0x04, 0x24, 0x68, 0xAC, 0xE0, 0x26, 0x0C, 0x00, 0x03, 0xEA, 0x34, 0x1F, 0x4D, 0xBC, 0x77, 0x80, 0x51, 0x11, 0x8E, 0xCD, 0x8D, 0x11, 0x8A, 0xD4, 0x7B, 0x00, 0x63, 0x6C, 0x00, 0x06 };

            //Int64 latInt = sendBytes[20] * 256 * 256 * 256 + sendBytes[21] * 256 * 256 + sendBytes[22] * 256 + sendBytes[23];
            //Console.WriteLine("latINT for 22.3344 ="+latInt);
            

            Byte[] lat_arr = Invers_Lat(-23.44556677);
            //setLAT
            sendBytes[23] = lat_arr[0];
            sendBytes[22] = lat_arr[1];
            sendBytes[21] = lat_arr[2];
            sendBytes[20] = lat_arr[3];

            
            //setLNG
            Byte[] lng_arr = Invers_LNG(-88.44556677);

            sendBytes[27] = lng_arr[0];
            sendBytes[26] = lng_arr[1];
            sendBytes[25] = lng_arr[2];
            sendBytes[24] = lng_arr[3];


            return sendBytes;
        }
    }
}