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

namespace SDRgateway
{
    class Parse_LRRP
    {
        public MOTO.GPS_POS ProcessCommand(byte[] Gpsbuf, uint len)
        {
            string ret ="";
            MOTO.GPS_POS gps_pos = null;

            // check for LRRP appl_id and DID
            if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_UNS_LOC_REP))
            {
                if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI_1)
                {
                    gps_pos = GpsUnsRep(Gpsbuf);
                }
                else
                {
                    Console.WriteLine("UNS_LOC_REP not TKNICE_REQID_OI_1");
                }
            }
            else if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_IMM_LOC_REP))
            {
                if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI)
                {
                    gps_pos = GpsImmResp(Gpsbuf);
                }
                else
                {
                    Console.WriteLine("IMM_LOC_REP not TKNICE_REQID_OI");
                }
            }
            else if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_TRI_LOC_ANS))
            {
                if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI)
                {
                    ret =GpsTrigStartResp(Gpsbuf);
                }
                else
                {
                    Console.WriteLine("TRI_LOC_ANS not TKNICE_REQID_OI");
                }
            }
            else if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_TRI_LOC_REP))
            {
                if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI)
                {
                    gps_pos = GpsTrigLoc(Gpsbuf);
                }
                else
                {
                    Console.WriteLine("TRI_LOC_REP not TKNICE_REQID_OI");
                }
            }
            else if ((Gpsbuf[0] == 0x80) && (Gpsbuf[1] == MOTO.DOCID_L_V1_TRI_STP_ANS))
            {
                if (Gpsbuf[3] == MOTO.TKNICE_REQID_OI)
                {
                    ret =GpsTrigStopResp(Gpsbuf);
                }
                else
                {
                    Console.WriteLine("TRI_STP_ANS not TKNICE_REQID_OI");
                }
            }
            else
            {
                Console.WriteLine("GPS data\n");
            }

            return gps_pos;
        }

        #region response type
        private MOTO.GPS_POS GpsUnsRep(byte[] Gpsbuf)
        {
            Console.WriteLine("GpsUnsRep \n");
            MOTO.GPS_POS ret_pos = new MOTO.GPS_POS() ;
            string ret_string;

            int index, max_index;
            max_index = Gpsbuf[2];
            index = 4;
            switch (Gpsbuf[index]) // Req_id Table 3 in LRRP Protocol Spec.
            {
                case 0x00: ret_string = string.Format("  Req_id Power On ");
                    break;
                case 0x07: ret_string = string.Format("  Req_id TMO On ");
                    break;
                case 0x10: ret_string = string.Format("  Req_id Leave Service ");
                    break;
                case 0x0F: ret_string = string.Format("  Req_id Loss of GPS coverage ");
                    break;
                case 0x2F: ret_string = string.Format("  Req_id Periodic report ");
                    break;
                default: ret_string = string.Format("  Req_id {0:X} ", Gpsbuf[index]);
                    break;

            }// end req_id

            index++;

            while (index - 2 < max_index)
            {
                switch (Gpsbuf[index])
                {
                    case MOTO.TKNIR_RESULT_OI_0:
                        ret_string += GpsErrorDec(ref index, Gpsbuf);
                        break;
                    case MOTO.TKNIR_INFO_TM_OI_5:
                        ret_pos.time = GpsTimeDec(ref index, Gpsbuf);
                        break;
                    case MOTO.TKNLR_POINT_2D:
                        ret_pos.pos = Gps2DDec(ref index, Gpsbuf);
                        break;
                    case MOTO.TKNLR_DIR_HOR_U8:
                        ret_string += GpsHorDec(ref index, Gpsbuf);
                        break;
                    case MOTO.TKNLR_LEV_CONF_U8:
                        ret_string += GpsLevDec(ref index, Gpsbuf);
                        break;
                    default:
                        break;
                }
                index++;

            }// end while

            return ret_pos;
        }


        private MOTO.GPS_POS GpsImmResp(byte[] Gpsbuf)
        {
            Console.WriteLine("GpsImmResp \n");
            string str;
            MOTO.GPS_POS ret_pos = new MOTO.GPS_POS();
            string ret_string = "";

            int index, max_index;
            max_index = Gpsbuf[2];
            index = 4;

            int req_len = Gpsbuf[index];
            Console.WriteLine(" max_index {0} req_len {1} \n", max_index, req_len);

            // compare TX req_id with this RX req_id
            if ((req_len == 4) && (Gpsbuf[index + 1] == 'A') &&
                 (Gpsbuf[index + 2] == 'n') && (Gpsbuf[index + 3] == 'n') &&
                 (Gpsbuf[index + 4] == 'e'))
            {
                Console.WriteLine("  Immediate Loc.Request Resp ");
                Console.WriteLine("  Req_id Anne");

                index = index + req_len + 1;
                while ((index - 2) < max_index)
                {
                    switch (Gpsbuf[index])
                    {
                        case MOTO.TKNIR_RESULT_OI_0:
                            string str_err = GpsErrorDec(ref index, Gpsbuf);
                            Console.WriteLine(str_err);
                            break;
                        case MOTO.TKNIR_INFO_TM_OI_5:
                            ret_pos.time = GpsTimeDec(ref index, Gpsbuf);
                            break;
                        case MOTO.TKNLR_POINT_2D:
                            ret_pos.pos = Gps2DDec(ref index, Gpsbuf);
                            break;
                        case MOTO.TKNLR_DIR_HOR_U8:
                            ret_string += GpsHorDec(ref index, Gpsbuf);
                            break;
                        case MOTO.TKNLR_LEV_CONF_U8:
                            ret_string += GpsLevDec(ref index, Gpsbuf);
                            break;
                        case MOTO.TKNLR_POINT_3D_2:
                            ret_pos.pos = Gps3DDec(ref index, Gpsbuf);
                            break;
                        default:
                            Console.WriteLine("GpsImmResp default...\n");
                            index = max_index + 2; //end while
                            break;
                    }
                }// end while

            }
            return ret_pos;
        }

        private string GpsTrigStartResp(byte[] Gpsbuf)
        {
            Console.WriteLine("GpsTrigStartResp \n");
            string str = "";

            int index, max_index;
            max_index = Gpsbuf[2];
            index = 4;

            int req_len = Gpsbuf[index];
            Console.WriteLine(" max_index %02X req_len %02X \n", max_index, req_len);

            // compare TX req_id with this RX req_id
            if ((req_len == 4) && (Gpsbuf[index + 1] == 'U') &&
                 (Gpsbuf[index + 2] == 'f') && (Gpsbuf[index + 3] == 'f') &&
                 (Gpsbuf[index + 4] == 'e'))
            {
                Console.WriteLine("  Trigger Start Request Resp ");

                index = index + req_len + 1;
                while ((index - 2) <= max_index)
                {
                    switch (Gpsbuf[index])
                    {
                        case MOTO.TKNIR_RESULT_0_OI_0: // result OK
                            string.Format("  Result code 0 ");
                            index++;
                            break;
                        case MOTO.TKNIR_RESULT_OI_0:
                            string str_err = GpsErrorDec(ref index, Gpsbuf); Console.WriteLine(str_err);
                            break;
                        default:
                            Console.WriteLine("GpsTrigStartResp default...\n");
                            index = max_index + 2; //end while
                            break;
                    }
                }// end while
            }

            return str;
        }

        private MOTO.GPS_POS GpsTrigLoc(byte[] Gpsbuf)
        {
            Console.WriteLine("GpsTrigLoc \n");
            string str;
            MOTO.GPS_POS ret_pos = new MOTO.GPS_POS();

            int index, max_index;
            max_index = Gpsbuf[2];
            index = 4;

            int req_len = Gpsbuf[index];
            Console.WriteLine(" max_index %02X req_len %02X \n", max_index, req_len);

            // compare TX req_id with this RX req_id
            if ((req_len == 4) && (Gpsbuf[index + 1] == 'U') &&
                 (Gpsbuf[index + 2] == 'f') && (Gpsbuf[index + 3] == 'f') &&
                 (Gpsbuf[index + 4] == 'e'))
            {
                Console.WriteLine("  Trigger Location ");


                index = index + req_len + 1;
                while ((index - 2) < max_index)
                {
                    switch (Gpsbuf[index])
                    {
                        case MOTO.TKNLR_POINT_3D_2:
                            ret_pos.pos = Gps3DDec(ref index, Gpsbuf);
                            break;
                        default:
                            Console.WriteLine("GpsTrigLoc default...\n");
                            index = max_index + 2; //end while
                            break;
                    }
                }// end while
            }
            return ret_pos;
        }

        private string GpsTrigStopResp(byte[] Gpsbuf)
        {
            Console.WriteLine("GpsTrigStopResp \n");
            string str = "";

            int index, max_index;
            max_index = Gpsbuf[2];
            index = 4;

            int req_len = Gpsbuf[index];
            Console.WriteLine(" max_index {0} req_len {1} \n", max_index, req_len);

            // compare TX req_id with this RX req_id
            if ((req_len == 4) && (Gpsbuf[index + 1] == 'U') &&
                 (Gpsbuf[index + 2] == 'f') && (Gpsbuf[index + 3] == 'f') &&
                 (Gpsbuf[index + 4] == 'e'))
            {
                Console.WriteLine("  Trigger Stop Request Resp ");

                index = index + req_len + 1;
                while ((index - 2) <= max_index)
                {
                    switch (Gpsbuf[index])
                    {
                        case MOTO.TKNIR_RESULT_0_OI_0: // result OK
                            string.Format("  Result code 0 ");
                            index++;
                            break;
                        case MOTO.TKNIR_RESULT_OI_0:
                            string str_err = GpsErrorDec(ref index, Gpsbuf); Console.WriteLine(str_err);
                            break;
                        default:
                            Console.WriteLine("GpsTrigStopResp default...\n");
                            index = max_index + 2; //end while
                            break;
                    }
                }// end while
            }
            return str;
        }

        #endregion
        string GpsErrorDec(ref int i, byte[] buf)
        {
            Console.WriteLine("GpsErrorDec 0x{0:X} \n", i);
            Console.WriteLine("GpsErrorDec 0x{0:X} 0x{1:X} 0x{2:X} 0x{3:X} 0x{4:X} 0x{5:X}\n",
                                buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
            string ret_string;
            i++;
            if (buf[i] == MOTO.TIME_EXPIRED)
            {
                ret_string = string.Format("  Operation Error - TIME EXPIRED");
            }
            else if (buf[i] == MOTO.NO_SUCH_REQUEST)
            {
                ret_string = string.Format("  Operation Error - NO_SUCH_REQUEST");
            }
            else if (buf[i] == MOTO.DUPLICATE_REQUEST_ID)
            {
                ret_string = string.Format("  Operation Error - DUPLICATE_REQUEST_ID");
            }
            else if (buf[i] == MOTO.IRRS_RES_SYNTAX_ERROR)
            {
                ret_string = string.Format("  Operation Error - SYNTAX_ERROR");
            }
            else
            {
                ret_string = string.Format("  Operation Error {0:X} ", buf[i]);
            }

            i++;
            return ret_string;
        }

        MOTO.irrs_con_time_t GpsTimeDec(ref int i, byte[] buf)
        {
            Console.WriteLine("GpsTimeDec {0:X} \n", i);

            MOTO.irrs_con_time_t conv_time;
            i++;
            conv_time.year = (buf[i] << 6) | (buf[i + 1] >> 2);
            conv_time.month = ((buf[i + 1] & 0x03) << 2) | (buf[i + 2] >> 6);
            conv_time.day = (buf[i + 2] >> 1) & 0x1F;
            conv_time.hour = ((buf[i + 2] & 0x01) << 4) | ((buf[i + 3] & 0xF0) >> 4);
            conv_time.minute = ((buf[i + 3] & 0xF) << 2) | ((buf[i + 4] & 0xC0) >> 6);
            conv_time.second = buf[i + 4] & 0x3F;

            Console.WriteLine("  Year {0:X} Month {1:X} Day {2:X} Hour {3:X} Minute {4:X} Second {5:X} \n",
                            conv_time.year, conv_time.month, conv_time.day, conv_time.hour,
                            conv_time.minute, conv_time.second);

            i = i + 5;

            return conv_time;
        }

        MOTO.lrrs_con_pos_t Gps2DDec(ref int i, byte[] buf)
        {
            Console.WriteLine("Gps2DDec {0} \n", i);
            i++;

            MOTO.lrrs_con_pos_t con_pos = new MOTO.lrrs_con_pos_t();

            //decode latitude
            decode_gps_position(ref con_pos,true, buf, i);
            Console.WriteLine("  Longitude {0} {1}° {2}", con_pos.direction_lat, con_pos.degrees_lat, con_pos.minutes_lat + con_pos.fraction_lat);

            i = i + 4;

            //decode longitude
            decode_gps_position(ref con_pos, false, buf, i);
            Console.WriteLine("  Latitude {0} {1}° {2}", con_pos.direction_lng, con_pos.degrees_lng, con_pos.minutes_lng + con_pos.fraction_lng);

            i = i + 4;

            return con_pos;
        }

        MOTO.lrrs_con_pos_t Gps3DDec(ref int i, byte[] buf)
        {
            Console.WriteLine("Gps3DDec {0} \n", i);
            i++;

            MOTO.lrrs_con_pos_t con_pos = new MOTO.lrrs_con_pos_t();

            //decode latitude
            decode_gps_position(ref con_pos, true, buf, i);
            Console.WriteLine("  Longitude {0} {1}° {2}", con_pos.direction_lat, con_pos.degrees_lat, con_pos.minutes_lat + con_pos.fraction_lat);

            i = i + 4;

            //decode longitude
            decode_gps_position(ref con_pos, false, buf, i);
            Console.WriteLine("  Latitude {0} {1}° {2}", con_pos.direction_lng, con_pos.degrees_lng, con_pos.minutes_lng + con_pos.fraction_lng);

            i = i + 4;

            //sintvar i.e. variable size, signed integer
            bool two_byte = ((buf[i] & 0x80) == 1) ? true : false; //continue
            bool positive = ((buf[i] & 0x40) == 1) ? true : false;
            int a = 0;
            a = buf[i] & 0x3F;
            i++;
            if (two_byte)
            {
                a = (a << 7 | buf[i]);
                i++;
            }
            if (a == 0)
                Console.WriteLine("  Altitude 0");
            else if (positive)
                Console.WriteLine("  Altitude -{0}", a);
            else
                Console.WriteLine("  Altitude {0}", a);

            //uintvar 
            string.Format("  Altitude Accuracy {0} meters", buf[i]);
            i++;

            return con_pos;
        }

        string GpsHorDec(ref int i, byte[] buf)
        {
            Console.WriteLine("GpsHorDec {0} \n", i);
            string str;
            i++;
            str = string.Format("GpsHorDec buf[i] {0} \n", buf[i]);
            if (buf[i] < 180)
            {
                str = string.Format("  Horizontal Direction {0} * 2 degrees ", buf[i]);

            }
            Console.WriteLine(str);
            i++;
            return str;
        }

        string GpsLevDec(ref int i, byte[] buf)
        {
            Console.WriteLine("GpsLevDec %02X \n", i);
            string str;
            i++;
            str = string.Format("  Level of Confidence %02d%% ", buf[i]);
            Console.WriteLine(str);
            i++;
            return str;
        }

        void decode_gps_position(ref MOTO.lrrs_con_pos_t lrrs_con_pos, bool bLat, byte[] buf, int i)
        {
            ulong degrees=0;
            ulong minutes=0;
            double fraction=0;
            double dbl;
            ulong ulat;
            bool non_neg_sign= false;
            // Converts the input string to an unsigned long, keeps track of the sign seperately.
            ulat = 0;
            for (int t = i; t < 4; ++t)
            {
                ulat = (ulat << 8) + buf[t];
            }
            if (bLat)//latitude
            {
                non_neg_sign = true;
                if ((ulat & MOTO.LRRS_LAT_SIGN_MSK) == 0)
                {
                    non_neg_sign = false;
                    ulat &= (~MOTO.LRRS_LAT_SIGN_MSK);
                }

                // Converts to double, including setting the sign.
                dbl = (double)ulat / MOTO.LRRS_LAT_MULTIPLIER;
                if (non_neg_sign == false)
                    dbl = -dbl;
            }
            else //longitude
            {
                // Convert the specified string into a unsigned long.
                if (ulat == 0x80000000)
                {
                    dbl = -MOTO.LRRS_MAX_LON_DBL;
                }
                else
                {
                    // Convert to double
                    dbl = (double)ulat / MOTO.LRRS_LON_MULTIPLIER;
                }
            }

            if (bLat)
                convert_to_con_components(dbl,ref non_neg_sign,
                                           ref degrees, ref minutes, ref fraction, MOTO.LRRS_MAX_LAT_INT);
            else
                convert_to_con_components(dbl, ref non_neg_sign,
                                           ref degrees, ref minutes, ref fraction, MOTO.LRRS_MAX_LON_INT);

            if (bLat)
            {
                if (non_neg_sign == true)
                    lrrs_con_pos.direction_lat = 'N';
                else
                    lrrs_con_pos.direction_lat = 'S';

                lrrs_con_pos.degrees_lat = degrees;
                lrrs_con_pos.minutes_lat = minutes;
                lrrs_con_pos.fraction_lat = fraction;
            }
            else
            {
                if (non_neg_sign == true)
                    lrrs_con_pos.degrees_lng = 'E';
                else
                    lrrs_con_pos.degrees_lng = 'W';

                lrrs_con_pos.degrees_lng = degrees;
                lrrs_con_pos.minutes_lng = minutes;
                lrrs_con_pos.fraction_lng = fraction;
            }
            

            //return lrrs_con_pos;
        }

        /*
 * Convert latitude or longitude information (double dbl
 * into the sign, degrees, minutes, and fraction parts.
 */
        bool
        convert_to_con_components(double dbl,
                                  ref bool non_neg_sign_ptr, ref ulong dgr_ptr,
                                  ref ulong minu_ptr, ref  double frc_ptr,
                                  ulong max_degrees)
        {

            // Convert the specified value into its absolute value, keeping track of the sign separetely.
            non_neg_sign_ptr = true;
            if (dbl < 0)
            {
                dbl = -dbl;
                non_neg_sign_ptr = false;
            }

            // Perform a quick validation against the worst case, to avoid  the potential for overflow conditions.
            if (dbl > ((double)(max_degrees + MOTO.LAT_LON_DOUBLE_ROUNDING)))
                return false;

            // Determine the values for the various components of the latitude or  longitude value.
            dgr_ptr = (ulong)dbl;
            dbl -= dgr_ptr;

            minu_ptr = (ulong)(dbl * MOTO.LRRS_LL_MIN_IN_DEG_INT);
            dbl -= ((double)minu_ptr / MOTO.LRRS_LL_MIN_IN_DEG_DBL);

            frc_ptr = dbl * MOTO.LRRS_LL_MIN_IN_DEG_DBL;

            // Handle any overflow condition. It is not likely that an overflow condition could exist.  The error checking is mostly "to be on the
            if (frc_ptr < 0.0)
                frc_ptr = 0.0;     /* rounding correction */
            if (frc_ptr > (1.0 - MOTO.LAT_LON_DOUBLE_ROUNDING))
                frc_ptr = 1.0 - MOTO.LAT_LON_DOUBLE_ROUNDING;   /* rounding correction */

            if (minu_ptr >= MOTO.LRRS_LL_MIN_IN_DEG_INT)
            {
                (minu_ptr) -= MOTO.LRRS_LL_MIN_IN_DEG_INT;
                (dgr_ptr)++;
            }

            if (dgr_ptr > max_degrees)
                return false;
            if (dgr_ptr == max_degrees)
            {
                if ((minu_ptr > 0) || (frc_ptr > MOTO.MAX_FRC_VARIANCE))
                    return false;
                frc_ptr = 0;
            }

            return true;
        }
    }
}