/***************************************************************************
 * Copyright (c) 2009 Digi International Inc., All Rights Reserved
 * 
 * This software contains proprietary and confidential information of Digi
 * International Inc.  By accepting transfer of this copy, Recipient agrees
 * to retain this software in confidence, to prevent disclosure to others,
 * and to make no use of this software other than that for which it was
 * delivered.  This is an unpublished copyrighted work of Digi International
 * Inc.  Except as permitted by federal law, 17 USC 117, copying is strictly
 * prohibited.
 * 
 * Restricted Rights Legend
 *
 * Use, duplication, or disclosure by the Government is subject to
 * restrictions set forth in sub-paragraph (c)(1)(ii) of The Rights in
 * Technical Data and Computer Software clause at DFARS 252.227-7031 or
 * subparagraphs (c)(1) and (2) of the Commercial Computer Software -
 * Restricted Rights at 48 CFR 52.227-19, as applicable.
 *
 * Digi International Inc. 11001 Bren Road East, Minnetonka, MN 55343
 *
 ***************************************************************************
 *
 * @Descr: Math functions
 *
 * @History:
 *
 ***************************************************************************/

namespace Digi.Common
{
    public class PolarCoordinate
    {
        public float radius;
        public float radians;

        public PolarCoordinate(float Radius, float Radians)
        {
            radius = Radius;
            radians = Radians;
        }

        public PolarCoordinate()
        {
            radius = 0;
            radians = 0;
        }

        public CartesianCoordinate ToCartesian()
        {
            return new CartesianCoordinate(radius * DigiMath.cos(radians), radius * DigiMath.sin(radians));
        }

        public void FromCartesian(float x, float y)
        {
            radius = DigiMath.sqrt(x * x + y * y);

            if (x == 0)
                radians = DigiMath.PIo2;
            else
                radians = DigiMath.abs(DigiMath.arctan(y / x));

            if (y < 0)
                radians = -radians;

        }
    }


    public class CartesianCoordinate
    {
        public float x;
        public float y;

        public CartesianCoordinate(float X, float Y)
        {
            x = X;
            y = Y;
        }

        public CartesianCoordinate()
        {
            x = 0;
            y = 0;
        }

        public PolarCoordinate ToPolar()
        {
            if (y < 0)
                return new PolarCoordinate(DigiMath.sqrt(x * x + y * y), -DigiMath.abs(DigiMath.arctan(y / x)));
            else
                return new PolarCoordinate(DigiMath.sqrt(x * x + y * y), DigiMath.abs(DigiMath.arctan(y / x)));
        }

        public void FromPolar(float Radius, float Radians)
        {
            x = Radius * DigiMath.cos(Radians);
            y = Radius * DigiMath.sin(Radians);
        }
    }

    public static class DigiMath
    {
        public const float PI = 3.14159F;
        public const float PIo2 = PI / 2;
        public const float F0 = 1;
        public const float F1 = 1;
        public const float F2 = 2;
        public const float F3 = 6;
        public const float F4 = 24;
        public const float F5 = 120;
        public const float F6 = 720;
        public const float F7 = 5040;
        public const float F8 = 40320;
        public const float F9 = 362880;
        public const float F10 = 3628800;
        public const float F11 = 39916800;
        //public const float F12 = 479001600;
        //public const float F13 = 6227020800;

        public static float sqrt(float x)
        {
            float a = 1;
            float b = 30;
            float m = 14.5F;
            float m2 = 210.25F;
            int i;

            if (x == 0) return 0;
            if (x == 1) return 1;
            if (x > 900) return -1; // this is an error state.

            // 30 will give us about 7 decimal places of accuracy
            for (i = 0; i < 10; i++)
            {
                if (m2 > x)
                {
                    b = m;
                    m = (a + b) / 2;
                    m2 = m * m;
                }
                else if (m2 < x)
                {
                    a = m;
                    m = (a + b) / 2;
                    m2 = m * m;
                }
                else if (m2 == x)
                {
                    return m;
                }
            }
            return m;
        }

        public static float abs(float x)
        {
            if (x < 0) return -x;
            return x;
        }

        public static int abs(int x)
        {
            if (x < 0) return -x;
            return x;
        }

        public static float distance(float x1, float y1, float x2, float y2)
        {
            float a = (x2 - x1);
            float b = (y2 - y1);
            return sqrt(a * a + b * b);
        }

        // trig functions
        public static float sin(float r)
        {
            float r2 = r * r;
            float r3 = r * r2;
            float r5 = r3 * r2;
            float r7 = r5 * r2;
            float r9 = r7 * r2;
            float r11 = r9 * r2;
            //float r13 = r11 * r2;

            return r - r3 / F3 + r5 / F5 - r7 / F7 + r9 / F9 - r11 / F11;// +r13 / F13;
        }

        public static float cos(float r)
        {
            float r2 = r * r;
            float r4 = r2 * r2;
            float r6 = r4 * r2;
            float r8 = r6 * r2;
            float r10 = r8 * r2;
            //float r12 = r10 * r2;

            return 1 - r2 / F2 + r4 / F4 - r6 / F6 + r8 / F8 - r10 / F10;// +r12 / F12;
        }

        public static float tan(float r)
        {
            return sin(r) / cos(r);
        }

        public static float arctan(float r)
        {
            // domain of arctan power series is abs(r) < 1
            // if this is violated then we apply a trick
            // arctan(x) = PI/2 - arctan(1/x)

            bool inverse = false;
            if (abs(r) >= 1)
            {
                r = 1 / r;
                inverse = true;
            }

            float r2 = r * r;
            float r3 = r * r2;
            float r5 = r3 * r2;
            float r7 = r5 * r2;
            float r9 = r7 * r2;
            float r11 = r9 * r2;
            //float r13 = r11 * r2;
            float result = r - r3 / 3 + r5 / 5 - r7 / 7 + r9 / 9 - r11 / 11;// +r13 / 13;

            if (inverse) result = PIo2 - result;

            return result;
        }


        // degrees/radians
        public static float ToDegrees(float radians)
        {
            return radians * 180 / PI;
        }

        public static float ToRadians(float degrees)
        {
            return degrees * PI / 180;
        }

        public static byte ReverseByte(byte b)
        {
            //byte rev = 0;

            b = (byte)(((b >> 1) & 0x55) | ((b << 1) & 0xaa));
            b = (byte)(((b >> 2) & 0x33) | ((b << 2) & 0xcc));
            b = (byte)(((b >> 4) & 0x0f) | ((b << 4) & 0xf0));

            //if ((b & 0x01) == 0x01) rev = (byte)(rev | 0x80);
            //if ((b & 0x02) == 0x02) rev = (byte)(rev | 0x40);
            //if ((b & 0x04) == 0x04) rev = (byte)(rev | 0x20);
            //if ((b & 0x08) == 0x08) rev = (byte)(rev | 0x10);
            //if ((b & 0x10) == 0x10) rev = (byte)(rev | 0x08);
            //if ((b & 0x20) == 0x20) rev = (byte)(rev | 0x04);
            //if ((b & 0x40) == 0x40) rev = (byte)(rev | 0x02);
            //if ((b & 0x80) == 0x80) rev = (byte)(rev | 0x01);

            return b;
        }

        public static int BuffToInt(byte[] buff, int index)
        {
            return BuffToInt(buff, index, true);
        }
        public static int BuffToInt(byte[] buff, int index, bool LSB)
        {
            int value;
            if (LSB)
            {
                value = buff[index];
                value |= buff[index + 1] << 8;
                value |= buff[index + 2] << 16;
                value |= buff[index + 3] << 24;
            }
            else
            {
                value = buff[index] << 24;
                value |= buff[index + 1] << 16;
                value |= buff[index + 2] << 8;
                value |= buff[index + 3];
            }
            return value;
        }

        public static uint BuffToUint(byte[] buff, int index)
        {
            return BuffToUint(buff, index, true);
        }
        public static uint BuffToUint(byte[] buff, int index, bool LSB)
        {
            uint value;
            if (LSB)
            {
                value = (uint)(buff[index]);
                value |= (uint)(buff[index + 1] << 8);
                value |= (uint)(buff[index + 2] << 16);
                value |= (uint)(buff[index + 3] << 24);
            }
            else
            {
                value = (uint)(buff[index] << 24);
                value |= (uint)(buff[index + 1] << 16);
                value |= (uint)(buff[index + 2] << 8);
                value |= (uint)(buff[index + 3]);
            }
            return value;
        }

        public static short BuffToShort(byte[] buff, int index)
        {
            return BuffToShort(buff, index, true);
        }
        public static short BuffToShort(byte[] buff, int index, bool LSB)
        {
            int value;
            if (LSB)
            {
                value = buff[index];
                value |= buff[index + 1] << 8;
            }
            else
            {
                value = buff[index] << 8;
                value |= buff[index + 1];
            }
            return (short)value;
        }

        public static void IntToBuff(byte[] buff, int index, int Value)
        {
            IntToBuff(buff, index, Value, true);
        }
        public static void IntToBuff(byte[] buff, int index, int Value, bool LSB)
        {
            if (LSB)
            {
                buff[index] = (byte)(0x000000FF & Value);
                buff[index + 1] = (byte)((0x0000FF00 & Value) >> 8);
                buff[index + 2] = (byte)((0x00FF0000 & Value) >> 16);
                buff[index + 3] = (byte)((0xFF000000 & Value) >> 24);
            }
            else
            {
                buff[index + 3] = (byte)(0x000000FF & Value);
                buff[index + 2] = (byte)((0x0000FF00 & Value) >> 8);
                buff[index + 1] = (byte)((0x00FF0000 & Value) >> 16);
                buff[index] = (byte)((0xFF000000 & Value) >> 24);
            }
        }

        public static void UintToBuff(byte[] buff, int index, uint Value, bool LSB)
        {
            if (LSB)
            {
                buff[index] = (byte)(0x000000FF & Value);
                buff[index + 1] = (byte)((0x0000FF00 & Value) >> 8);
                buff[index + 2] = (byte)((0x00FF0000 & Value) >> 16);
                buff[index + 3] = (byte)((0xFF000000 & Value) >> 24);
            }
            else
            {
                buff[index + 3] = (byte)(0x000000FF & Value);
                buff[index + 2] = (byte)((0x0000FF00 & Value) >> 8);
                buff[index + 1] = (byte)((0x00FF0000 & Value) >> 16);
                buff[index] = (byte)((0xFF000000 & Value) >> 24);
            }
        }

        public static void ShortToBuff(byte[] buff, int index, short Value)
        {
            ShortToBuff(buff, index, Value, true);
        }
        public static void ShortToBuff(byte[] buff, int index, short Value, bool LSB)
        {
            if (LSB)
            {
                buff[index] = (byte)(0x000000FF & (int)Value);
                buff[index + 1] = (byte)((0x0000FF00 & (int)Value) >> 8);
            }
            else
            {
                buff[index + 1] = (byte)(0x000000FF & (int)Value);
                buff[index] = (byte)((0x0000FF00 & (int)Value) >> 8);
            }

        }
    }


}