#include <IRremote.h>  //versione libreria 3.5.0
#include <JC_Button.h>

#include <LiquidCrystal.h>
#include <Servo.h>

class ServoMotion {
  public:
    ServoMotion(uint8_t pin) : m_pin(pin) {
        // costruttore vuoto
    }

    // Restituisce la posizione corrente in gradi
    uint8_t getPos() const {
        return m_currPos;
    }

    // Imposta la posizione da raggiungere.
    // Nota che se la condizione (m_currPos != pos && m_isFinish)
    // è false non verrà impostata alcuna posizione.
    void setPos(int pos) {
        if (m_currPos != pos && m_isFinish) {
            m_sign = (m_currPos - pos < 0) ?  true : false;
            m_isFinish = false;
            m_targetPos = pos;
            m_saveTime = 0;
            m_speed0 = 0;
            // Se il servo è scollegato lo collega
            if (!m_srv.attached())
                m_srv.attach(m_pin);
        }
    }

    // Imposta la velocità in millesimi di secondo per ogni grado
    void setSpeed(uint32_t ms) {
        m_speed1 = ms;
    }
    
    // Imposta la velocità in gradi/secondo
    void setSpeed_gs(uint8_t gs) {
        //          1000 / gs  -> più veloce
        m_speed1 = (1500 / gs); // -> meno veloce
    }
    // restituisce true quando ha raggiunto la posizione
    // desiderata.
    bool isFinished() const {
        return m_isFinish;
    }
    // Se tf = true il servo resta collegato dopo avere raggiunto
    // la posizione desiderata.
    // Se tf = false (default), il servo si scollega
    // dopo avere raggiunto la posizione desiderata.
    void setKeepPos(bool tf) {
        m_keepPos = tf;
    }
    // Chiama run() in loop alla massima frequenza possibile.
    void run() {
        if (m_isFinish == false) {
            if (millis() - m_saveTime > m_speed0) {
                m_saveTime = millis();
                m_speed0 = m_speed1;
                if (m_sign) {
                    m_srv.write(m_currPos++);
                } else {
                    m_srv.write(m_currPos--);
                }
                if (m_srv.read() == m_targetPos) {
                    m_isFinish = true;
                    // scollega il servo quando ha finito
                    // se m_keepPos è false
                    if (!m_keepPos) 
                        m_srv.detach();
                }
            }
        }
    }

  private:
    uint8_t m_pin;
    bool m_isFinish = true;
    uint32_t m_saveTime;
    uint8_t m_speed0;
    uint8_t m_speed1;
    bool m_sign;
    bool m_keepPos;
    int16_t m_targetPos;
    int16_t m_currPos = 90;
    Servo m_srv;
};

// hardware buttons
#define BTN_POWER     4 

#define SRV0_PIN  5
#define SRV0_POS 90
ServoMotion srv0(SRV0_PIN);

// IR & IR commands
// Signal Pin of IR receiver
#define PIN_RECEIVER  2   
#define IRCMD_POWER   162

Button btnPower(BTN_POWER);
IRrecv receiver(PIN_RECEIVER);

LiquidCrystal lcd(12, 11, 10, 9, 8, 7);

uint32_t rndTimer;
uint16_t rndInterval;

void bswap(uint8_t &a, uint8_t &b) {
  
  int c = a;
  a = b;
  b = c;

}

void setup() {
    Serial.begin(115200);
 
    receiver.enableIRIn(); // Start the receiver

    lcd.begin(16, 2);
    btnPower.begin();
    
    srv0.setSpeed_gs(40);
    srv0.setPos(SRV0_POS);
    
    while (!btnPower.read());
    // Inizializza il generatore rnd
    randomSeed(micros());

    Serial.println(srv0.isFinished());
}

void loop() {
    btnPower.read();
    uint8_t command = 0;
    srv0.run();

    if (receiver.decode()) {
        command = receiver.decodedIRData.command;
        receiver.resume();  // Receive the next value
        Serial.println(command);
    } 

    if (srv0.isFinished()) { 
        if (millis() - rndTimer > rndInterval) {
            rndTimer = millis();
            
            // genera posizione
            uint8_t grrnd = random(20, 160);
            // calcola velocita in gradi/secondo
            uint8_t speed = abs((srv0.getPos() - grrnd));
            // genera intervallo
            rndInterval = random(2000, 7000);

            Serial.print("pos:          ");
            Serial.println(grrnd);
            Serial.print("rndInterval:  ");
            Serial.println(rndInterval);
            Serial.print("speed g/s:    ");
            Serial.println(speed);
            srv0.setSpeed_gs(speed);
            srv0.setPos(grrnd);
        }
    }
}