#include "mbed.h"

PwmOut servo(PC_7);
PwmOut us_1_trigger(PB_13);
PwmOut us_2_trigger(PB_14);
PwmOut us_3_trigger(PB_15);

InterruptIn s1_pos1(PA_10);  //Schalter definieren
InterruptIn s2_pos2(PA_6); 
InterruptIn s3_pos3(PA_1);
InterruptIn s4_stop(PB_0);
InterruptIn us_1_echo(PB_5);
InterruptIn us_2_echo(PB_4);
InterruptIn us_3_echo(PB_10);

PortOut motor(PortC, 0xf00); // Schrittmotor definieren

int m_state[4] = {0x300, 0x700, 0xc00, 0x900}; // Ansteuerung der Spuhlen in Schrittmotor

Ticker stepperTicker;
Timer us_1_timer;
Timer us_2_timer;
Timer us_3_timer;

int volatile us_1_cm;
int volatile us_2_cm;
int volatile us_3_cm;

unsigned int const T_periode = 20; //Variablen definieren

volatile int steps = 0;

void isr_us_1_echo_rise(){
    us_1_timer.start();
}

void isr_us_1_echo_fall(){
    us_1_timer.stop();
    us_1_cm = us_1_timer.elapsed_time().count() /58;
    us_1_timer.reset();
    
}

void isr_us_2_echo_rise(){
    us_2_timer.start();
}

void isr_us_2_echo_fall(){
    us_2_timer.stop();
    us_2_cm = us_2_timer.elapsed_time().count() /58;
    us_2_timer.reset();
}

void isr_us_3_echo_rise(){
    us_3_timer.start();
}

void isr_us_3_echo_fall(){
    us_3_timer.stop();
    us_3_cm = us_3_timer.elapsed_time().count() /58;
    if (us_3_cm <= 5){
        
    }
    us_3_timer.reset();
}

void moveServoToPosition(int position) {
    switch (position) {
        case 1:
            servo.pulsewidth_us(500);
            break;
        case 2:
            servo.pulsewidth_us(1500);
            break;
        case 3:
            servo.pulsewidth_us(2500);
            break;
        default:
            break;
    }
}

void stepperCallback() {
    if (steps > 0) {
        motor = m_state[3 - steps % 4];
        steps--;
        if (steps <= 1){
            steps = -100;
        }
    }else if (steps < 0){
        motor = m_state[(-steps) % 4];
        steps++;
    }else{
        motor = 0x000;
    }
}

void isr_s1_pos1() {
    steps = 0;
    moveServoToPosition(1);
    steps = 50000;
}

void isr_s2_pos2() {
    steps = 0;
    moveServoToPosition(2);
    steps = 50000;
}

void isr_s3_pos3() {
    steps = 0;
    moveServoToPosition(3);
    steps = 50000;
}

void isr_s4_stop(){
    steps = -100;
    moveServoToPosition(2);
}

int main() {  //Wenn Taster gedrückt ist, soll dies/das passieren und Tasterfunktionen werden definiert
    s1_pos1.mode(PullDown);
    s1_pos1.rise(&isr_s1_pos1);

    s2_pos2.mode(PullDown);
    s2_pos2.rise(&isr_s2_pos2);

    s3_pos3.mode(PullDown);
    s3_pos3.rise(&isr_s3_pos3);

    s4_stop.mode(PullDown);
    s4_stop.rise(&isr_s4_stop);

    servo.period_ms(T_periode);

    us_1_trigger.period_ms(200);
    us_1_trigger.pulsewidth_ms(1);

    us_2_trigger.period_ms(200);
    us_2_trigger.pulsewidth_ms(1);

    us_3_trigger.period_ms(200);
    us_3_trigger.pulsewidth_ms(1);

    us_1_echo.rise(&isr_us_1_echo_rise);
    us_1_echo.fall(&isr_us_1_echo_fall);

    us_2_echo.rise(&isr_us_2_echo_rise);
    us_2_echo.fall(&isr_us_2_echo_fall);

    us_3_echo.rise(&isr_us_3_echo_rise);
    us_3_echo.fall(&isr_us_3_echo_fall);

    stepperTicker.attach(&stepperCallback, 4ms); //Timer-Intervall wird voreingestellt

    while (true) {}
}