#include "ServoLifterDriver.h"

ServoLifterDriver sld(9);

void Enable(){
  if(!sld.GetEnabled()){
    Serial.print("Enable ");
    if(sld.CanSetEnabled(true)){
      Serial.println("OK");
      sld.SetEnabled(true);
    } else {
      Serial.println("Fail");
    }
  }
}

bool playing = false;
void TonePlay(long freq){
  static long lastFreq = 0;
  if(playing && lastFreq == freq) return;
  lastFreq = freq;
  tone(8, freq);
  playing = true;
}

void ToneStop(){
  noTone(8);
  playing = false;
}

void WaitSetState(LifterState newState, bool verbose = false){
  if(sld.GetState() == newState){
    if(verbose) Serial.println("Done");
    return;
  }
  if(sld.CanSetState(newState)){    
    sld.SetState(newState);
    if(verbose) Serial.print("Waiting... ");
    
    long moveStart = millis();
    long moveEnd = moveStart + 400;

    const long upFreq = 960;
    const long downFreq = 64;

    long startFreq = newState == Up ? downFreq : upFreq;
    long endFreq = newState == Up ? upFreq : downFreq;

    
    while(sld.GetState() != newState){
      long freq = map(millis(), moveStart, moveEnd, startFreq, endFreq);
      TonePlay(freq);
      delayMicroseconds(50); 
    }
    if(verbose) Serial.println("Done");
    ToneStop();
  } else {
    if(verbose) Serial.println("Failed");
  }
}

void Raise(){
  Serial.print("Raise ");
  WaitSetState(Up, true);
}

void Lower(){
  Serial.print("Lower ");
  WaitSetState(Down, true);
}

void setup() {
  Serial.begin(115200);
  sld.Init();  
}

void loop() {
  Enable();
  
  Lower();
  delay(random(200,1000));

  Raise();
  delay(random(200,1500));
}