#include <HCSR04.h>
#include "Bsp.h"
#include "Motor.h"
#include "CH452.h"
#include "stm32f10x_tim.h"
#include "timer.h"

int Motor_PWM_1 = 0;
int Motor_PWM_2 = 0;
int s1 = 0;
int s2 = 0;
int s3 = 0;
int s4 = 0;
int s5 = 0;
int s6 = 0;
int s7 = 0;
int s8 = 0;
int s9 = 0;
int s10 = 0;
int s11 = 0;
int s12 = 0;



void GoStraight(void) {
    delay_ms(10);
    Set_Motor1_RPM(180);
    Set_Motor2_RPM(180);
}

void TurnLeft(void) {
    delay_ms(10);
    Set_Motor1_RPM(100);
    Set_Motor2_RPM(180);
    delay_ms(10);
}

void TurnRight(void) {
    delay_ms(10);
    Set_Motor1_RPM(180);
    Set_Motor2_RPM(100);
    delay_ms(10);
}

void StopMotors(void) {
    Set_Motor1_RPM(0);
    Set_Motor2_RPM(0);
}

// Put distanceSensor1 on the right of automation car
UltraSonicDistanceSensor distanceSensor1(A4, A5); // Change it to the correct pin A4 = trig, A5 = echo
// Put distanceSensor2 on the middle of automation car
UltraSonicDistanceSensor distanceSensor2(A6, A7); // Change it to the correct pin A6 = trig, A7 = echo
// Put distanceSensor3 on the back of automation car
UltraSonicDistanceSensor distanceSensor3(A8, A9); // Change it to the correct pin A8 = trig, A9 = echo

void setup() {
    Serial.begin(9600); 
    BSP_Init();
}

void loop() {
    float distance1 = distanceSensor1.measureDistanceCm();
    float distance2 = distanceSensor2.measureDistanceCm();
    float distance3 = distanceSensor3.measureDistanceCm();
    Serial.println(distance1);
    Serial.println(distance2);
    Serial.println(distance3);
    s1 = Read_IO1;
    s2 = Read_IO2;
    s3 = Read_IO3;
    s4 = Read_IO4;
    s5 = Read_IO5;
    s6 = Read_IO6;
    s7 = Read_IO7;
    s8 = Read_IO8;
    s9 = Read_IO9;
    s10 = Read_IO10;
    s11 = Read_IO11;
    s12 = Read_IO12;
    delay(500);

    if ((distance1 >= 10) && (s6 == 1 || s7 == 1 || s8 == 1 || s5 == 1)) {
        GoStraight();
    }
    else if ((distance2 <= 4) && (s11 == 1 || s12 == 1)) {
        TurnRight();
    }
    else if ((distance2 > 4) && (distance1 >= 10) && (s1 == 1 || s2 == 1)) {
        TurnLeft();
    }
    else if (distance1 < 10) {
        TurnRight();
        if (distance3 == 10) {
            GoStraight();
        }
        else if (distance3 == 5) {
            // Put program of hand robot
        }
        else if (distance2 <= 4) {
            // Put program for moveback
        }
    }
}