#include "mbed.h"
#include <chrono>

PwmOut servo(PC_7);
InterruptIn s1_pos1(PA_10);
InterruptIn s2_pos2(PA_6);
InterruptIn s3_pos3(PA_1);

PortOut motor(PortC, 0xf00);

int m_state[4] = {0x300, 0x700, 0xc00, 0x900};

Timer timer;

unsigned int const T_periode = 20;

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 m_pump() {
    timer.start();
    while (std::chrono::duration_cast<std::chrono::milliseconds>(timer.elapsed_time()).count() < 15000) {
        for (int i = 0; i < 4; i++) {
            motor = m_state[i];
            ThisThread::sleep_for(4ms);
        }
    }
        for (int j = 0; j < 100; j++) {
            for (int i = 3; i >= 0; i--) {
                motor = m_state[i];
                ThisThread::sleep_for(4ms);
            }
        }
        motor = 0x000;
        timer.stop();
        timer.reset();
    
}

void isr_s1_pos1() {
    moveServoToPosition(1);
    m_pump();
}

void isr_s2_pos2() {
    moveServoToPosition(2);
    m_pump();
}

void isr_s3_pos3() {
    moveServoToPosition(3);
    m_pump();
}

int main() {
    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);

    servo.period_ms(T_periode);

    while (true) {
    }
}