#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 set(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) {
        m_speed1 = (1500 / gs);
    }
    // restituisce true quando ha raggiunto la posizione
    // desiderata.
    bool isFinished() const {
        return m_isFinish;
    }

    void setKeepPos(bool tf) {
        m_keepPos = tf;
    }



    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;

  
  int m_targetPos;
  int 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);

uint8_t srv0Pos[] = { 90, 165, 96, 90, 15, 88, 75, 90 };
uint8_t posIdx = 0;

uint32_t rndTimer;
uint16_t rndInterval;

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

}

void generatePos() {
    
    for (uint8_t i = 0; i < sizeof(srv0Pos); i++) {
        //uint8_t p0 = srv0Pos[i];
        //uint8_t p1 = srv0Pos[i+1]+1;
        //if (p0 > p1)
        //  bswap(p0, p1);
        uint8_t rnd = random(0, 180);
        //if (rnd > 179) 
        //    rnd = 180;
        srv0Pos[i+1] = rnd;
    }
}

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

    lcd.begin(16, 2);
    btnPower.begin();
    
    srv0.setSpeed_gs(70);
    srv0.set(90);
    //Serial.println(srv0.isFinished());

    while (!btnPower.read());
    // Inizializza il generatore rnd
    randomSeed(micros());

    Serial.println(srv0.isFinished());
    /*for (uint8_t i = 0; i < sizeof(srv0Pos); i++) {
        Serial.println(srv0Pos[i]);
    }
    generatePos();
    for (uint8_t i = 0; i < sizeof(srv0Pos); i++) {
        Serial.println(srv0Pos[i]);
    }
    generatePos();
    for (uint8_t i = 0; i < sizeof(srv0Pos); i++) {
        Serial.println(srv0Pos[i]);
    }
    generatePos();
    for (uint8_t i = 0; i < sizeof(srv0Pos); i++) {
        Serial.println(srv0Pos[i]);
    }
    generatePos();
    for (uint8_t i = 0; i < sizeof(srv0Pos); i++) {
        Serial.println(srv0Pos[i]);
    }*/



    /*int cpos = 90;
    int tpos = 180;
    bool sign = (tpos - cpos < 0) ? true : false;
    Serial.println(sign);*/

}

void movie() {
    if (srv0.isFinished()) { 
        if (posIdx < sizeof(srv0Pos)) {

            srv0.set(srv0Pos[posIdx]);
            posIdx++;
        } else {
            generatePos();
            for (uint8_t i = 0; i < sizeof(srv0Pos); i++) {
                Serial.println(srv0Pos[i]);
            }
            posIdx = 0;
        }
    }

}

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(0, 180);
          // 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.set(grrnd);
      }

           
        
    }
    
}