#include <Arduino_FreeRTOS.h>
#include <task.h>




#include <Servo.h>

#define  ECHO_PIN 2
#define  TRIG_PIN 3
#define pinBuzzer 8
#define pinBuzzer2 4


Servo myservo;
int rotasi = 90;
int interval = 0;

int DP;

const int pinLedR1 = 12;
const int pinLedY1 = 11;
const int pinLedG1 = 10;

const int pinLedR2 = 7;
const int pinLedY2 = 6;
const int pinLedG2 = 5;

const int pinLedR3 = A2;
const int pinLedY3 = A1;
const int pinLedG3 = A0;

const int pinLedR4 = A5;
const int pinLedY4 = A4;
const int pinLedG4 = A3;

const int pinLedR5 = 4;
const int pinLedY5 = 1;
const int pinLedG5 = 0;

float readDistanceCM(){
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);
  int duration = pulseIn(ECHO_PIN, HIGH);
  return duration * 0.034/2;
}



void soundBuzzer(){
  if(interval < 500){
    tone(pinBuzzer, 250);
  }else
  if(interval > 500){
    tone(pinBuzzer, 100,800);
  };
}

void soundBuzzer2(){

   // tone(pinBuzzer2, 250);

    tone(pinBuzzer2, 100,500);
    delay(500); 
    tone(pinBuzzer2, 200,1500);
    delay(1000); 
    tone(pinBuzzer2, 100,500);
    delay(500); 
    tone(pinBuzzer2, 200,1500);
    delay(1000); 
    tone(pinBuzzer2, 100,500);
    delay(500); 
    tone(pinBuzzer2, 200,1500);
    delay(1000); 
}


void task1(void *pvParameters) {
  for (;;) {
    lalulintas();
    vTaskDelay(50 / portTICK_PERIOD_MS);
  }
}

void task2(void *pvParameters) {
  for (;;) {
 
    
DP=digitalRead(13);
    
if(DP==0){
   digitalWrite (pinLedR5,HIGH);
   digitalWrite (pinLedY5,HIGH);
   digitalWrite (pinLedG5,LOW);
  
   delay(1000);      
   digitalWrite (pinLedR5,HIGH);
   digitalWrite (pinLedY5,LOW);
   digitalWrite (pinLedG5,LOW);
    soundBuzzer2();
   delay(5000);      
    }else{

    digitalWrite (pinLedR5,LOW);
    digitalWrite (pinLedY5,LOW);
    digitalWrite (pinLedG5,HIGH);   

    }                  
    vTaskDelay(50 / portTICK_PERIOD_MS);
  }
}

void task3(void *pvParameters) {



  for (;;) {

    tutupPalang();
 interval +=100;
 if(interval>1000) interval = 0;

    vTaskDelay(50 / portTICK_PERIOD_MS);
  }
}




void setup(){
  
pinMode (pinLedR1,OUTPUT); // init pin led 1 sebagai output
pinMode (pinLedY1,OUTPUT); // init pin led 2 sebagai output
pinMode (pinLedG1,OUTPUT); // init pin led 3 sebagai output
pinMode (pinLedR2,OUTPUT); // init pin led 4 sebagai output
pinMode (pinLedY2,OUTPUT); // init pin led 5 sebagai output
pinMode (pinLedG2,OUTPUT); // init pin led 6 sebagai output
pinMode (pinLedR3,OUTPUT); // init pin led 7 sebagai output
pinMode (pinLedY3,OUTPUT); // init pin led 8 sebagai output
pinMode (pinLedG3,OUTPUT); // init pin led 9 sebagai output
pinMode (pinLedR4,OUTPUT); // init pin led 10 sebagai output
pinMode (pinLedY4,OUTPUT); // init pin led 11 sebagai output
pinMode (pinLedG4,OUTPUT); // init pin led 12 sebagai output

pinMode (pinLedR5,OUTPUT); // init pin led 13 sebagai output
pinMode (pinLedY5,OUTPUT); // init pin led 14 sebagai output
pinMode (pinLedG5,OUTPUT); // init pin led 15 sebagai output

pinMode(13, INPUT_PULLUP);


  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  Serial.begin(115200);
  pinMode(pinBuzzer, OUTPUT);
  myservo.attach(9);
  myservo.write(90);



  
  xTaskCreate(task1, "Task1", 128, NULL, 1, NULL);
  xTaskCreate(task2, "Task2", 128, NULL, 2, NULL);
  xTaskCreate(task3, "Task3", 128, NULL, 3, NULL);
 

  // Start scheduler
  vTaskStartScheduler();

}

void loop(){}


void lalulintas()
{
digitalWrite (pinLedR1,HIGH);
digitalWrite (pinLedY1,LOW);
digitalWrite (pinLedG1,LOW);
digitalWrite (pinLedR2,HIGH);
digitalWrite (pinLedY2,LOW);
digitalWrite (pinLedG2,LOW);
digitalWrite (pinLedR3,HIGH);
digitalWrite (pinLedY3,LOW);
digitalWrite (pinLedG3,LOW);
digitalWrite (pinLedR4,LOW);
digitalWrite (pinLedY4,LOW);
digitalWrite (pinLedG4,HIGH);
delay(5000);

digitalWrite (pinLedR1,HIGH);
digitalWrite (pinLedY1,LOW);
digitalWrite (pinLedG1,LOW);
digitalWrite (pinLedR2,HIGH);
digitalWrite (pinLedY2,LOW);
digitalWrite (pinLedG2,LOW);
digitalWrite (pinLedR3,HIGH);
digitalWrite (pinLedY3,LOW);
digitalWrite (pinLedG3,LOW);
digitalWrite (pinLedR4,LOW);
digitalWrite (pinLedY4,HIGH);
digitalWrite (pinLedG4,HIGH);
delay(1000);


digitalWrite (pinLedR1,LOW);
digitalWrite (pinLedY1,LOW);
digitalWrite (pinLedG1,HIGH);
digitalWrite (pinLedR2,HIGH);
digitalWrite (pinLedY2,LOW);
digitalWrite (pinLedG2,LOW);
digitalWrite (pinLedR3,HIGH);
digitalWrite (pinLedY3,LOW);
digitalWrite (pinLedG3,LOW);
digitalWrite (pinLedR4,HIGH);
digitalWrite (pinLedY4,LOW);
digitalWrite (pinLedG4,LOW);
delay(5000);

digitalWrite (pinLedR1,LOW);
digitalWrite (pinLedY1,HIGH);
digitalWrite (pinLedG1,HIGH);
digitalWrite (pinLedR2,HIGH);
digitalWrite (pinLedY2,LOW);
digitalWrite (pinLedG2,LOW);
digitalWrite (pinLedR3,HIGH);
digitalWrite (pinLedY3,LOW);
digitalWrite (pinLedG3,LOW);
digitalWrite (pinLedR4,HIGH);
digitalWrite (pinLedY4,LOW);
digitalWrite (pinLedG4,LOW);
delay(1000);

digitalWrite (pinLedR1,HIGH);
digitalWrite (pinLedY1,LOW);
digitalWrite (pinLedG1,LOW);
digitalWrite (pinLedR2,HIGH);
digitalWrite (pinLedY2,LOW);
digitalWrite (pinLedG2,LOW);
digitalWrite (pinLedR3,LOW);
digitalWrite (pinLedY3,LOW);
digitalWrite (pinLedG3,HIGH);
digitalWrite (pinLedR4,HIGH);
digitalWrite (pinLedY4,LOW);
digitalWrite (pinLedG4,LOW);
delay(5000);

digitalWrite (pinLedR1,HIGH);
digitalWrite (pinLedY1,LOW);
digitalWrite (pinLedG1,LOW);
digitalWrite (pinLedR2,HIGH);
digitalWrite (pinLedY2,LOW);
digitalWrite (pinLedG2,LOW);
digitalWrite (pinLedR3,LOW);
digitalWrite (pinLedY3,HIGH);
digitalWrite (pinLedG3,HIGH);
digitalWrite (pinLedR4,HIGH);
digitalWrite (pinLedY4,LOW);
digitalWrite (pinLedG4,LOW);
delay(1000);

digitalWrite (pinLedR1,HIGH);
digitalWrite (pinLedY1,LOW);
digitalWrite (pinLedG1,LOW);
digitalWrite (pinLedR2,LOW);
digitalWrite (pinLedY2,LOW);
digitalWrite (pinLedG2,HIGH);
digitalWrite (pinLedR3,HIGH);
digitalWrite (pinLedY3,LOW);
digitalWrite (pinLedG3,LOW);
digitalWrite (pinLedR4,HIGH);
digitalWrite (pinLedY4,LOW);
digitalWrite (pinLedG4,LOW);
delay(5000);

digitalWrite (pinLedR1,HIGH);
digitalWrite (pinLedY1,LOW);
digitalWrite (pinLedG1,LOW);
digitalWrite (pinLedR2,LOW);
digitalWrite (pinLedY2,HIGH);
digitalWrite (pinLedG2,HIGH);
digitalWrite (pinLedR3,HIGH);
digitalWrite (pinLedY3,LOW);
digitalWrite (pinLedG3,LOW);
digitalWrite (pinLedR4,HIGH);
digitalWrite (pinLedY4,LOW);
digitalWrite (pinLedG4,LOW);
delay(1000);
}






void tutupPalang()
{
  float jarak = readDistanceCM();
  if(jarak > 0 && jarak < 100 ){
    soundBuzzer();
    tutup();
  }else{
    buka();
    noTone(pinBuzzer);
  }
}

void tutup()
{
  if(rotasi <= 90){
    myservo.write(rotasi);
    delay(100);
    rotasi--;
  }
  if(rotasi <= 0 ) rotasi = 0;
}

void buka()
{
  if(rotasi <= 90){
    myservo.write(rotasi);
    delay(100);
    rotasi+=4;
  }
  if(rotasi >= 90 ) rotasi = 90;
}