#include "Ultrasonic.h"
#include <Servo.h>
#include "PID_v1.h"

// Konstanta
#define TRIGGER_PIN 12
#define ECHO_PIN 13
#define SETPOINT_PIN A0
#define SERVO_PIN 3
#define ALARM_PIN 4

// Variable PID
double setpoint, input, output;
double Kp = 2, Ki = 0, Kd = 0; // P-Only, yang lain dibuat 0

Ultrasonic ultrasonic(TRIGGER_PIN, ECHO_PIN);
Servo srv;
PID pidcs(&input, &output, &setpoint, Kp, Ki, Kd, P_ON_E, DIRECT);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  Serial.println(__FILE__);

  pinMode(ALARM_PIN, OUTPUT);
  digitalWrite(ALARM_PIN, LOW);

  // define limit of pid
  pidcs.SetOutputLimits(-4, 255);

  // Nyalakan PID
  pidcs.SetMode(AUTOMATIC);

  // inisiasi nilai awal pembacaan
  input = ultrasonic.read(CM);

  // Actuator
  srv.attach(SERVO_PIN);
}

// Print ke serial monitor
void report(void) {
  static uint32_t last = 0;
  const int interval = 1000; // interval proses print

  if (millis() - last > interval) {
    last += interval;

    Serial.print("SP:");
    Serial.print(setpoint);
    Serial.print(" PV:");
    Serial.print(input);
    Serial.print(" CV:");
    Serial.print(output);
    Serial.print(' ');
    Serial.println();
  }
}

void loop() {

  input = ultrasonic.read(CM);

  if (pidcs.Compute()) {
    int sudut = map((int) output, 0, 255, 0, 180);
    srv.write(sudut);

    if (sudut < 100) {
      digitalWrite(ALARM_PIN, HIGH);
    } else {
      digitalWrite(ALARM_PIN, LOW);
    }

    // Interpolasi 0 - 1023 ---> 2 - 400
    setpoint = map(analogRead(SETPOINT_PIN), 0, 1023, 2, 400);
  }

  report();
}