// =============================================================================================
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.IO.Ports;
using System.Runtime.InteropServices;
using System.Reflection;
// =============================================================================================
namespace AltinoSerial
{
    class ALTINO                                // Use "ALTINO." to access thind in this class
    {
        public class AltinoSensor01             // Altino --> PC command 1 [31 Bytes]
        {
            public long[] IR = new long[6];     // IR 6ea [7-18]
            public long MoTQR;                  // Motor TQ - Right [19-20]
            public long MoTQL;                  // Motor TQ - Left [21-22]
            public long Temp;                   // Temp [23-24]
            public long CDS;                    // CDS [25-26]
            public long Remote;                 // Remote control [27]
        };
        public class AltinoSensor02             // Altino --> PC command 2 [31 Bytes]
        {
            public long[] AccG = new long[3];   // Acceleration Sensor 3ea X,Y,Z [7-12]
            public long[] Mag = new long[3];    // Magnetic Field Sensor 3ea X,Y,Z [13-18]
            public long SteerVar;               // Steer Register Value [19-20]
            public long SteerMoTQ;              // Steer Motor TQ [21-22]
            public long Batt;                   // Battery [23-24]
            public long MTemp;                  // Temp of Magnetic sensor [25]
        };
        public class AltinoSensor10             // Altino --> PC command 10 [56 Bytes]
        {
            public long[] IR = new long[6];     // IR 6ea [7-18]
            public long[] AccG = new long[3];   // Acceleration Sensor 3ea  X,Y,Z [25-30]
            public long[] Mag = new long[3];    // Magnetic Field Sensor 3ea X,Y,Z [31-36]
            public long[] Gyro = new long[3];   // Gyro 3 axis [37-42]
            public long MoTQR;                  // Motor TQ - Right [19-20]
            public long MoTQL;                  // Motor TQ - Left [21-22]
            public long SteerMoTQ;              // Steer Motor TQ [23-24]
            public long CDS;                    // CDS [43-44]
            public long SteerVal;               // Steer Register Value [45-46]
            public long Batt;                   // Battery [47-48]
            public long Temp;                   // Temp [49-50]
            public long Remote;                 // Remote control [51]
            public long MTemp;                  // Temp of Magnetic sensor [52]
        };
        // -------------------------------------------------------------------------------------
        public static AltinoSensor10 aParseSensor56(byte[] senData56)
        {
            AltinoSensor10 atinoSen10 = new AltinoSensor10();

            atinoSen10.IR[0] = senData56[7] * 256 + senData56[8];
            atinoSen10.IR[1] = senData56[9] * 256 + senData56[10];
            atinoSen10.IR[2] = senData56[11] * 256 + senData56[12];
            atinoSen10.IR[3] = senData56[13] * 256 + senData56[14];
            atinoSen10.IR[4] = senData56[15] * 256 + senData56[16];
            atinoSen10.IR[5] = senData56[17] * 256 + senData56[18];

            atinoSen10.MoTQR = senData56[19] * 256 + senData56[20];
            atinoSen10.MoTQL = senData56[21] * 256 + senData56[22];
            atinoSen10.Temp = senData56[49] * 256 + senData56[50];

            atinoSen10.CDS = senData56[43] * 256 + senData56[44];

            atinoSen10.AccG[0] = (long)senData56[25] * 256 + (long)senData56[26];
            atinoSen10.AccG[1] = (long)senData56[27] * 256 + (long)senData56[28];
            atinoSen10.AccG[2] = (long)senData56[29] * 256 + (long)senData56[30];

            long temp = 0;

            temp = atinoSen10.AccG[0] / 16;
            atinoSen10.AccG[0] = temp;
            if (temp > 2048) atinoSen10.AccG[0] = atinoSen10.AccG[0] - 4096;

            temp = atinoSen10.AccG[1] / 16;
            atinoSen10.AccG[1] = temp;
            if (temp > 2048) atinoSen10.AccG[1] = atinoSen10.AccG[1] - 4096;

            temp = atinoSen10.AccG[2] / 16;
            atinoSen10.AccG[2] = temp;
            if (temp > 2048) atinoSen10.AccG[2] = atinoSen10.AccG[2] - 4096;

            atinoSen10.Mag[0] = (long)senData56[31] * 256 + (long)senData56[32];
            atinoSen10.Mag[1] = (long)senData56[33] * 256 + (long)senData56[34];
            atinoSen10.Mag[2] = (long)senData56[35] * 256 + (long)senData56[36];

            atinoSen10.SteerVal = senData56[45] * 256 + senData56[46];
            atinoSen10.SteerMoTQ = senData56[23] * 256 + senData56[24];
            atinoSen10.Batt = senData56[47] * 256 + senData56[48];
            atinoSen10.Remote = senData56[51] * 256 + senData56[52];

            atinoSen10.Gyro[0] = (long)senData56[37] * 256 + (long)senData56[38];
            atinoSen10.Gyro[1] = (long)senData56[39] * 256 + (long)senData56[40];
            atinoSen10.Gyro[2] = (long)senData56[41] * 256 + (long)senData56[42];

            if (atinoSen10.AccG[0] > 32768) atinoSen10.AccG[0] = atinoSen10.AccG[0] - 65535;
            if (atinoSen10.AccG[1] > 32768) atinoSen10.AccG[1] = atinoSen10.AccG[1] - 65535;
            if (atinoSen10.AccG[2] > 32768) atinoSen10.AccG[2] = atinoSen10.AccG[2] - 65535;
            if (atinoSen10.Mag[0] > 32768) atinoSen10.Mag[0] = atinoSen10.Mag[0] - 65535;
            if (atinoSen10.Mag[1] > 32768) atinoSen10.Mag[1] = atinoSen10.Mag[1] - 65535;
            if (atinoSen10.Mag[2] > 32768) atinoSen10.Mag[2] = atinoSen10.Mag[2] - 65535;
            if (atinoSen10.Gyro[0] > 32768) atinoSen10.Gyro[0] = atinoSen10.Gyro[0] - 65535;
            if (atinoSen10.Gyro[1] > 32768) atinoSen10.Gyro[1] = atinoSen10.Gyro[1] - 65535;
            if (atinoSen10.Gyro[2] > 32768) atinoSen10.Gyro[2] = atinoSen10.Gyro[2] - 65535;

            return atinoSen10;
        }
        // -------------------------------------------------------------------------------------
        // 다른 곳에서 호출해서 사용하려면 "public static" 로 선언해야함
        // -------------------------------------------------------------------------------------
        public static byte CheckSum56(byte[] rx_d56)
        {
            byte rxCheckSum = 0;

            rxCheckSum = rx_d56[0];
            rxCheckSum += rx_d56[1];
            for (int i = 3; i < 56; i++) rxCheckSum += rx_d56[i];
            rxCheckSum = (byte)(rxCheckSum % 256);
            return rxCheckSum;
        }
        // -------------------------------------------------------------------------------------
        public static byte CheckSum31(byte[] rx_d31)
        {
            byte rxCheckSum = 0;

            rxCheckSum = rx_d31[0];
            rxCheckSum += rx_d31[1];
            for (int i = 3; i < 31; i++) rxCheckSum += rx_d31[i];
            rxCheckSum = (byte)(rxCheckSum % 256);
            return rxCheckSum;
        }
        // -------------------------------------------------------------------------------------
        public static int[] tIndex = { 0, 50, 100, 150, 200, 250, 300, 350, 400, 450, 500, 550, 600, 650, 700, 750, 800, 850, 900, 950, 1024 };
        public static int[] phyTemp = { 104, 95, 86, 77, 68, 60, 51, 44, 36, 30, 25, 21, 17, 11, 2, -6, -14, -21, -28, -35, -40 };
        public static int[] IRindex = { 40, 70, 120, 250, 950};
        public static int[] phyIR = { 150, 70, 50, 30, 10 };        // distance = mm
        // -------------------------------------------------------------------------------------
        public static long Convert2Physical(int[] tNdx, int[] PhyVal, long Val2Find)
        {
            int i = 0;
            if (Val2Find > tNdx[tNdx.Length - 1]) return PhyVal[tNdx.Length - 1];
            if (Val2Find < tNdx[0]) return PhyVal[0];
            while (i < tNdx.Length && Val2Find > tNdx[i]) i++;
            double x0 = tNdx[i - 1];
            double x1 = tNdx[i];
            double y0 = PhyVal[i - 1];
            double y1 = PhyVal[i];
            double RealValue = y0 + (Val2Find - x0) / (x1 - x0) * (y1 - y0);
            return (long) RealValue;
        }
        // -------------------------------------------------------------------------------------
        // leftMost = 130(45도), rightMost = 930(45도), CenterValue = 530
        // -------------------------------------------------------------------------------------
        public static String SteerAngle(long SteerVal)
        {
            long steerAngle = 0;
            long CenterValue = 530;
            if (SteerVal == CenterValue) return ("C00");
            if (SteerVal > 930) SteerVal = 930;
            if (SteerVal < 130) SteerVal = 130;
            steerAngle = (CenterValue - SteerVal) * 45 / 400;
            if(steerAngle > 0) return ("L"+ steerAngle.ToString());
            else return ("R" + (steerAngle*-1).ToString());
        }
        // -------------------------------------------------------------------------------------
        // -------------------------------------------------------------------------------------
        // =====================================================================================
    }
}