#include <stdint.h>
#include <map>
#include "Arduino.h"

// Parallel Implementation of the HX711 for the ESP32

class HX711ll
{
public:
    HX711ll(uint32_t sckPin, uint32_t gain = 128)
    {
        m_isRunning = false;
        m_sckPin = sckPin;
        setGain(gain);
    }

    virtual ~HX711ll()
    {
    }

    bool Add(const uint32_t dataPin)
    {
        bool success = false;
        if (m_isRunning == false)
        {
            m_sensors[dataPin] = 0;
            success = true;
        }
        return success;
    }

    bool Del(const uint32_t dataPin)
    {
        bool success = false;
        if (m_isRunning == false)
        {
            m_sensors.erase(dataPin);
            success = true;
        }
        return success;
    }

    void Start()
    {
        pinMode(m_sckPin, OUTPUT);
        std::map<uint32_t, uint32_t>::iterator it = m_sensors.begin();
        while (it != m_sensors.end())
        {
            Serial.println(it->first);
            pinMode(it->first, INPUT);
            it++;
        }
        digitalWrite(m_sckPin, LOW);
        m_isRunning = true;
    }

    void Stop()
    {
        digitalWrite(m_sckPin, LOW);
        digitalWrite(m_sckPin, HIGH);
        m_isRunning = false;
    }

    bool Read(uint32_t timeout = 100)
    {
        bool success = false;
        if (m_isRunning == true)
        {
            Serial.println("RUNNING");
            if (checkAllReady() == true)
            {
                Serial.println("READY");
                digitalWrite(m_sckPin, HIGH);
                delayMicroseconds(1);
                std::map<uint32_t, uint32_t>::iterator it = m_sensors.begin();
                while (it != m_sensors.end())
                {
                    uint32_t sval = 0;
                    for (uint32_t i = 0; i < 24; i++)
                    {
                        sval |= digitalRead(it->first) << (31 - i);
                    }
                    it->second = sval;
                    it++;
                }
                digitalWrite(m_sckPin, LOW);
                delayMicroseconds(1);

                for (unsigned int i = 0; i < m_gain; i++)
                {
                    digitalWrite(m_sckPin, HIGH);
                    delayMicroseconds(1);
                    digitalWrite(m_sckPin, LOW);
                    delayMicroseconds(1);
                }
                success = true;
            }
        }
        return success;
    }

    uint32_t Get(const uint32_t dataPin)
    {
        return m_sensors[dataPin];
    }

private:
    void setGain(uint32_t gain)
    {
        switch (gain)
        {
        case 128: // channel A, gain factor 128
            m_gain = 1;
            break;
        case 64: // channel A, gain factor 64
            m_gain = 3;
            break;
        case 32: // channel B, gain factor 32
            m_gain = 2;
            break;
        }
    }

    bool checkAllReady()
    {
        bool ready = true;
        std::map<uint32_t, uint32_t>::iterator it = m_sensors.begin();
        while (it != m_sensors.end())
        {
            if (isReady(it->first) == false)
            {
                Serial.println(it->first);
                ready = false;
                break;
            }
            it++;
        }
        return ready;
    }

    static bool isReady(const uint32_t dataPin)
    {
        return digitalRead(dataPin) == LOW;
    }

private:
    bool m_isRunning;
    uint32_t m_sckPin;
    std::map<uint32_t, uint32_t> m_sensors;
    uint32_t m_gain;
};

static HX711ll scale(16);

void setup() {
  Serial.begin(9600);
  Serial.println("Initializing the scale");
  
  scale.Add(4);

  scale.Start();
}

void loop() {
  Serial.println("LOOP");
  if (scale.Read() == true)
  {
    float m1 = map(scale.Get(4), 0, 2100, 0, 5000);
    float m2 = map(scale.Get(17), 0, 2100, 0, 5000);
    Serial.print(m1);
    Serial.print(" - ");
    Serial.println(m2);
  }
  delay(1000);
}