using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using ANT_Managed_Library;
using System;

public class SpeedSensorReceiver
{
    private int deviceId = 0; //set this to connect to a specific device ID
    private bool connected = false; //will be set to true once connected
    private float wheelCircumference = 2.096f; //in meters, 700*23C, set this to your wheels size
    private List<AntDevice> scanResult;
    
    private float speed; // The speed in m/s
    private float distance; //the distance in meters


    private AntChannel backgroundScanChannel;
    private AntChannel deviceChannel;

    private int stopRevCounter_speed = 0;
    private int prev_measTime_speed = 0;
    private int prev_revCount_speed = 0;
    private int revCountZero = 0;

    public int DeviceId => deviceId;

    public bool Connected => connected;

    public List<AntDevice> ScanResult => scanResult;

    public float Speed => speed;
    
    public float SpeedKmh => speed * 3.6f;

    public float Distance => distance;

    public SpeedSensorReceiver()
    {
    }

    public SpeedSensorReceiver(int deviceId) => this.deviceId = deviceId;

    public SpeedSensorReceiver(int deviceId, float wheelCircumference)
    {
        this.deviceId = deviceId;
        this.wheelCircumference = wheelCircumference;
    }

    //Start a background Scan to find the device
    public void StartScan()
    {
        Debug.Log("Looking for ANT + Speed sensor");
        AntManager.Instance.Init();
        scanResult = new List<AntDevice>();
        backgroundScanChannel = AntManager.Instance.OpenBackgroundScanChannel(0);
        backgroundScanChannel.onReceiveData += ReceivedBackgroundScanData;
    }

    //If the device is found
    void ReceivedBackgroundScanData(Byte[] data)
    {
        byte deviceType = (data[12]); // extended info Device Type byte

        switch (deviceType)
        {
            case AntplusDeviceType.BikeSpeed:
            {
                int deviceNumber = (data[10]) | data[11] << 8;
                byte transType = data[13];
                foreach (AntDevice d in scanResult)
                {
                    if (d.deviceNumber == deviceNumber && d.transType == transType) //device already found
                        return;
                }


                AntDevice foundDevice = new AntDevice();
                foundDevice.deviceType = deviceType;
                foundDevice.deviceNumber = deviceNumber;
                foundDevice.transType = transType;
                foundDevice.period = 8118;
                foundDevice.radiofreq = 57;
                foundDevice.name = "BikeSpeed(" + foundDevice.deviceNumber + ")";
                scanResult.Add(foundDevice);
                if (deviceNumber == deviceId)
                {
                    Debug.Log($"Desired Speed Sensor with id {deviceNumber} found!");
                    ConnectToDevice(foundDevice);
                }
                else
                {
                    Debug.Log($"Speed sensor ({deviceNumber}) found and added to ScanResult");
                }

                break;
            }
        }
    }

    void ConnectToDevice(AntDevice device)
    {
        AntManager.Instance.CloseBackgroundScanChannel();
        byte channelID = AntManager.Instance.GetFreeChannelID();
        deviceChannel = AntManager.Instance.OpenChannel(ANT_ReferenceLibrary.ChannelType.BASE_Slave_Receive_0x00,
            channelID, (ushort) device.deviceNumber, device.deviceType, device.transType, (byte) device.radiofreq,
            (ushort) device.period, false);
        connected = true;
        deviceChannel.onReceiveData += Data;
        deviceChannel.onChannelResponse += ChannelResponse;

        deviceChannel.hideRXFAIL = true;
    }

    //Deal with the received Data
    private void Data(Byte[] data)
    {
        //SPEED
        int measTime_speed = (data[4]) | data[5] << 8;
        int revCount_speed = (data[6]) | data[7] << 8;


        if (prev_measTime_speed != 0 && measTime_speed != prev_measTime_speed && prev_measTime_speed < measTime_speed &&
            prev_revCount_speed < revCount_speed)
        {
            speed = (wheelCircumference * (revCount_speed - prev_revCount_speed) * 1024) /
                    (measTime_speed - prev_measTime_speed);
            stopRevCounter_speed = 0;
        }
        else
            stopRevCounter_speed++;

        if (stopRevCounter_speed >= 5)
        {
            stopRevCounter_speed = 5;
            speed = 0;
        }


        prev_measTime_speed = measTime_speed;
        prev_revCount_speed = revCount_speed;

        //DISTANCE
        if (revCountZero == 0)
            revCountZero = revCount_speed;

        distance = wheelCircumference * (revCount_speed - revCountZero);
    }


    void ChannelResponse(ANT_Response response)
    {
    }
}