using System;
using System.Threading;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using SWModules;

namespace HWModules
{
    public sealed class LheBoard
    {
        private static LheBoard instance;
        private static LhePio pio1, pio2;
        private static SMBusController smbus;

        /// <summary>
        /// Battery information array.
        /// </summary>
        /// <see
        public static BatteryState[] BatteryBank;

        private LheBoard()  
        {
            //LHE Requires 5V
            var mb = MotherBoard.Instance;
            mb.Enable5V();

            var clock_rate = 20;
            pio1 = new LhePio(new I2CDevice.Configuration(0x21, clock_rate), 6);
            pio2 = new LhePio(new I2CDevice.Configuration(0x20, clock_rate), 6);

            var batt_i2c = new I2CDevice.Configuration(0x16 >> 1, clock_rate);

            smbus = new SMBusController(batt_i2c, 6);

            BatteryBank = new BatteryState[6];

            Thread.Sleep(200);


            InitPIO();

            UpdateBatteryBank();
        }

        /// <summary>
        /// Does a round robin collection of data from each battery in the
        /// pack, collecting critical information and stores it in the object's
        /// <c>BatteryBank</c> field.
        /// </summary>
        /// <returns>true on success</returns>
        /// <see cref="BatteryBank"/>
        public bool UpdateBatteryBank()
        {
            //Check Sequencer
            var time_sleep = 0; //ms
            var embus = EnergyMonitorHotel.Instance; 
            EnergyMonitorLTC2946.EM_VandI vandi;

            for (var n = 0; n < 6; n++)
            {

                //POLL INPUTS FOR CHARGER STATUS
                Thread.Sleep(time_sleep);
                var charging_batt = CurrentBatteryUnderCharge;
                if (charging_batt == -1)
                {
                    EngrLogger.Comment("[ERROR] LHEBoard::UpdateBatteryPack Couldn't Poll PIO 0x20 for Charge Sequence");
                    vandi = embus.getVoltsAndMilliAmps();
                    return false;
                }

                if (charging_batt == n + 1)
                {
                    //Debug.Print("[ERROR] LHEBoard::UpdateBatteryPack Can't check battery, it's charging");
                    BatteryBank[n].Charging = true;
                    continue;
                }

                Thread.Sleep(time_sleep);

                //Debug.Print("[INFO] Selecting Battery: "+n);

                //Switch on I2C to Battery
                if (!SelectBatteryI2C(n + 1))
                {
                    EngrLogger.Comment("[ERROR] LHEBoard::UpdateBatteryPack Couldn't Enable PIO 0x21 for Battery Update");
                    vandi = embus.getVoltsAndMilliAmps();
                    return false;
                }

                Thread.Sleep(time_sleep);
                smbus.SetModeDefaults();
                Thread.Sleep(time_sleep);
                BatteryBank[n].Voltage = smbus.ReadVoltage();
                Thread.Sleep(time_sleep);
                BatteryBank[n].Current = (short) smbus.ReadCurrent();
                Thread.Sleep(time_sleep);
                BatteryBank[n].SerialNumber = smbus.ReadSerialNumber();
                Thread.Sleep(time_sleep);
                BatteryBank[n].RemainingCapacity = smbus.ReadRemainingCapacity();
                Thread.Sleep(time_sleep);
                BatteryBank[n].Temperature = smbus.ReadTemperature();
                Thread.Sleep(time_sleep);
                BatteryBank[n].BatteryStatus = smbus.ReadBatteryStatus();
                Thread.Sleep(time_sleep);
                BatteryBank[n].BatteryMode = smbus.ReadBatteryMode();
                Thread.Sleep(time_sleep);
                BatteryBank[n].Charging = false;

                //Disable Battery Communication
                if (!DisableBatteryI2C())
                {
                    EngrLogger.Comment("[ERROR] LHEBoard::UpdateBatteryPack Couldn't Poll PIO 0x21 for Switch Disable");
                    vandi = embus.getVoltsAndMilliAmps();
                    //Debug.Print(vandi.ToString());
                    return false;
                }

            }

            return true;
        }

        /// <summary>
        /// BatteryHealth returns an uint16 with bit flags irregularites
        /// in Mode or Status flags:
        ///   bits [0..5]: 1 if battery's mode value is not 0x4000
        ///   bits [6..11]: 1 if battery's status is not 0x00C0 or 0x00E0
        /// </summary>
        public ushort BatteryHealth
        {
            get
            {
                ushort health = 0;
                for (var n = 0; n < 6; n++)
                {
                    if (BatteryBank[n].BatteryMode != 0x4000)
                        health |= (ushort)(0x01 << n);
                    if (BatteryBank[n].BatteryStatus != 0x00E0 || BatteryBank[n].BatteryMode != 0x00C0)
                        health |= (ushort) (0x01 << (n + 6));
                }

                return health;
            }
        }

        /// <summary>
        /// Print all battery data to the EngrLogger.
        /// </summary>
        /// <see cref="EngrLogger"/>
        public void PrintBatteryData()
        {
            //Debug.Print("--------------------------------------------------------------");
            for (var n = 0; n < 6; n++)
            {
                EngrLogger.Comment(BatteryBank[n].ToString());
            }
        }

        
        /// <summary>
        /// Interrogate which battery is charging
        /// </summary>
        /// <returns>0-6 value of which battery is charging, 0 is no battery.</returns>
        public int CurrentBatteryUnderCharge
        {
            get {
                byte ret = 0;
                if (!pio2.readRegister(LhePio.Command.InputPort, ref ret))
                    return -1;

                return 0x07 & ret;
            }
        }

        /// <summary>
        /// Interrogate if the Pump power is is not faulted
        /// </summary>
        /// <returns>1 for NOT FAULTED, 0 for FAULTED, and -1 for a bit read error.</returns>
        public int isPumpPowerNotFaulted
        {
            get
            {
                byte ret = 0;
                if (!pio2.readRegister(LhePio.Command.InputPort, ref ret))
                    return -1;

                return ((0x08 & ret) == 0x08)?1:0;
            }
        }

        /// <summary>
        /// This sets up the parallel IO chips on the LHE for the designed
        /// IO on the LHE board. It also resets the sleep circuit, and important
        /// step to make sure the system doesn't accidentally put itself to sleep
        /// after returning from a sleep cycle. 
        /// </summary>
        private void InitPIO()
        {
            _currentPio1Output = DefaultPio1Output;
            _currentPio2Output = DefaultPio2Output;

            //RESETTING SLEEP FUNCTIONS ASSERTS CONFIGURATION MODES AS WELL
            ResetSleepFunctions();

        }

        private const LhePio.RegOutput DefaultPio1Output = LhePio.RegOutput.O0 |
                                             LhePio.RegOutput.O1 |
                                             LhePio.RegOutput.O2 |
                                             LhePio.RegOutput.O3 |
                                             LhePio.RegOutput.O4 |
                                             LhePio.RegOutput.O5;
                                             //0b 0011 1111
        private const LhePio.RegConfiguration DefaultPio1Config = (LhePio.RegConfiguration) 0x80; // P7 Set to input
        private const LhePio.RegConfiguration DefaultPio2Config = (LhePio.RegConfiguration) 0xFF; // All inputs
        private const LhePio.RegConfiguration SleepTimerPio1Config = 0; //All outputs
        private const LhePio.RegConfiguration SleepTimerPio2Config = (LhePio.RegConfiguration) 0x1F; //P5-7 outputs

        private const LhePio.RegOutput DefaultPio2Output = 0;

        private LhePio.RegOutput _currentPio1Output;
        private LhePio.RegOutput _currentPio2Output;

        public static LheBoard Instance
        {
            get { return instance ?? (instance = new LheBoard()); }
        }


        /// <summary>
        /// Disable all analog switches that facilitate SMBus
        /// communication with individual battery packs. 
        /// </summary>
        /// <returns></returns>
        public bool DisableBatteryI2C()
        {
            var tempOutput = _currentPio1Output;
            tempOutput |= LhePio.RegOutput.O0;
            tempOutput |= LhePio.RegOutput.O1;
            tempOutput |= LhePio.RegOutput.O2;
            tempOutput |= LhePio.RegOutput.O3;
            tempOutput |= LhePio.RegOutput.O4;
            tempOutput |= LhePio.RegOutput.O5;
            if (!pio1.setOutputs(tempOutput))
                return false;
            _currentPio1Output = tempOutput;
            return true;
        }

        /// <summary>
        /// Use the PIO1 chip to single device communication.
        /// </summary>
        /// <param name="p_n"></param>
        /// <returns>true on success</returns>
        public bool SelectBatteryI2C(int p_n)
        {
            //if (!DisableBatteryI2C())
            //{
                //return false;
            //}
            var tempOutput = _currentPio1Output;
            //reset all outputs for battery select
            tempOutput |= (LhePio.RegOutput) 0x3F;

            switch (p_n)
            {
                case 1: //P1
                    tempOutput &= ~LhePio.RegOutput.O0;
                    break;
                case 2:
                    tempOutput &= ~LhePio.RegOutput.O1;
                    break;
                case 3:
                    tempOutput &= ~LhePio.RegOutput.O2;
                    break;
                case 4:
                    tempOutput &= ~LhePio.RegOutput.O3;
                    break;
                case 5:
                    tempOutput &= ~LhePio.RegOutput.O4;
                    break;
                case 6:
                    tempOutput &= ~LhePio.RegOutput.O5;
                    break;
                default:
                    return false;
            }

            // standard: tempOutput || all on 0x80 || all off 0xBF
            if (!pio1.setOutputs(tempOutput)) 
                return false;

            _currentPio1Output = tempOutput;
            return true;
        }

        /// <summary>
        /// Enables/Disables the Battery power supply to the Elmo
        /// Twitter
        /// </summary>
        /// <param name="enable"><c>true</c> enables, <c>false</c> disables</param>
        /// <returns><c>true</c> on success</returns>
        public bool EnablePumpPower(bool enable)
        {
            var tempOutput = _currentPio1Output;
            if (enable)
                tempOutput |= LhePio.RegOutput.O6;
            else
                tempOutput &= ~LhePio.RegOutput.O6;

            if (!pio1.setOutputs(tempOutput)) return false;
            _currentPio1Output = tempOutput;
            return true;
        }

        /// <summary>
        /// Loads the register held by the flip-flop with value
        /// 0x0 to 0x7. This will sleep for 2^n-1 minutes.
        /// </summary>
        /// <param name="n">Valid range, <c>0x0-0x7</c></param>
        /// <returns><c>true</c> on success</returns>
        public bool SetupSleep(byte n)
        {
            //Test n
            if (n > 0x7) return false;

            var tempPio1Output = _currentPio1Output;
            var tempPio2Output = _currentPio2Output;

            //Set SleepTrig to 0

            tempPio1Output &= ~LhePio.RegOutput.O7;
            if (!pio1.setOutputs(tempPio1Output)) return false;
            _currentPio1Output = tempPio1Output;

            //Set ABC to 0
            tempPio2Output = DefaultPio2Output;
            if (!pio2.setOutputs(tempPio2Output)) return false;
            _currentPio2Output = tempPio2Output;

            //Configure SleepTrig Outputs
            if (!pio1.setPortDirections(SleepTimerPio1Config)) return false;
            if (!pio2.setPortDirections(SleepTimerPio2Config)) return false;

            //Set ABC
            if ((n & 0x1) > 0x00)
                tempPio2Output |= LhePio.RegOutput.O5;
            if ((n & 0x2) > 0x00)
                tempPio2Output |= LhePio.RegOutput.O6;
            if ((n & 0x4) > 0x00)
                tempPio2Output |= LhePio.RegOutput.O7;

            if (!pio2.setOutputs(tempPio2Output)) return false;
            _currentPio2Output = tempPio2Output;

            return true;

        }

        /// <summary>
        /// Engages the sleep function of the LHE board for minutes, calls <see cref="SetupSleep(byte)"/>.
        /// </summary>
        /// <param name="n">Valid range, <c>0x0-0x7</c></param>
        /// <returns>true on success</returns>
        public bool Sleep(byte n)
        {
            if (!SetupSleep(n)) return false;

            //Enable the sleep trigger
            //Latch ABC via SleepTrig
            var tempPio1Output = _currentPio1Output;
            tempPio1Output |= LhePio.RegOutput.O7;
            if (!pio1.setOutputs(tempPio1Output)) return false;
            _currentPio1Output = tempPio1Output;

            return true;
        }

        /// <summary>
        /// Conducts an output sequence of the PIO chips that resets the sleep functions
        /// circuits on the LHE board. 
        /// </summary>
        /// <returns></returns>
        public bool ResetSleepFunctions()
        {

            if (!Sleep(0)) return false;
            // reset line configs
            if (!pio1.setPortDirections(DefaultPio1Config)) return false;
            return pio2.setPortDirections(DefaultPio2Config);
        }

    }

    /// <summary>
    /// Command interface for the TI TCA6408APWR 8-bit I2C I/O expander
    /// </summary>
    /// <see href="https://www.ti.com/store/ti/en/p/product/?p=TCA6408APWR"/>
    public class LhePio : I2CPortBase
    {
        public enum Command : byte
        {
            InputPort = 0x00,
            OutputPort = 0x01,
            PolarityInversion = 0x02,
            Configuration = 0x03
        }


        public enum Direction
        {
            Input,
            Output
        }

        [Flags]
        public enum RegConfiguration : byte
        {
            C0 = 0x01,
            C1 = 0x02,
            C2 = 0x04,
            C3 = 0x08,
            C4 = 0x10,
            C5 = 0x20,
            C6 = 0x40,
            C7 = 0x80,
        }

        [Flags]
        public enum RegInput : byte
        {
            I0 = 0x01,
            I1 = 0x02,
            I2 = 0x04,
            I3 = 0x08,
            I4 = 0x10,
            I5 = 0x20,
            I6 = 0x40,
            I7 = 0x80
        }

        [Flags]
        public enum RegInversion : byte
        {
            N0 = 0x01,
            N1 = 0x02,
            N2 = 0x04,
            N3 = 0x08,
            N4 = 0x10,
            N5 = 0x20,
            N6 = 0x40,
            N7 = 0x80,
        }

        [Flags]
        public enum RegOutput : byte
        {
            O0 = 0x01,
            O1 = 0x02,
            O2 = 0x04,
            O3 = 0x08,
            O4 = 0x10,
            O5 = 0x20,
            O6 = 0x40,
            O7 = 0x80,
        }

        private byte _configurationReg;

        private byte _inputReg;
        private byte _outputReg;
        private byte _polarityReg;
        private byte[] _readBytes;
        private const int I2CTimeout = 500;

        private I2CDevice.I2CTransaction[] _transactions;
        private I2CDevice.I2CTransaction[] _write_transaction;
        private byte[] _writeByte;
        private byte[] _writeBytes;

        public LhePio(I2CDevice.Configuration config, int slotNum) : base(config, slotNum)
        {
            _transactions = new I2CDevice.I2CTransaction[2];
            _write_transaction = new I2CDevice.I2CTransaction[1];
            _writeByte = new byte[1];
            _writeBytes = new byte[2];
            _readBytes = new byte[1];
        }

        public bool readRegister(Command command, ref byte value)
        {
            _writeByte[0] = (byte) command;
            _transactions[0] = CreateWriteTransaction(_writeByte);
            _transactions[1] = CreateReadTransaction(_readBytes);

            if (Execute(_transactions, I2CTimeout) == 0)
            {
                //Debug.Print("[ERROR] Reading");
                return false;
            }

            switch (command)
            {
                case Command.InputPort:
                    _inputReg = _readBytes[0];
                    break;
                case Command.OutputPort:
                    _outputReg = _readBytes[0];
                    break;
                case Command.PolarityInversion:
                    _polarityReg = _readBytes[0];
                    break;
                case Command.Configuration:
                    _configurationReg = _readBytes[0];
                    break;
                default:
                    throw new ArgumentOutOfRangeException("command");
            }

            value = _readBytes[0];
            return true;
        }

        public bool setPortDirections(RegConfiguration configuration)
        {
            _writeBytes[0] = (byte) Command.Configuration;
            _writeBytes[1] = (byte) configuration;
            _write_transaction[0] = CreateWriteTransaction(_writeBytes);

            if (Execute(_write_transaction, I2CTimeout) > 0) return true;

            EngrLogger.Comment("[Error] LHEBoard::setPortDirections Writing Port Direction.");
            return false;
        }



        public bool setOutputs(RegOutput outputs)
        {
            _writeBytes[0] = (byte) Command.OutputPort;
            _writeBytes[1] = (byte) outputs;
            _write_transaction[0] = CreateWriteTransaction(_writeBytes);
            
            
            if (Execute(_write_transaction, I2CTimeout) > 0) return true;

            EngrLogger.Comment("[ERROR] LHEBoard::setOutPuts Writing Outputs: "+_writeBytes[1].ToString("X"));
            return false;
        }
    }
}