#include "mbed.h"
#include "DriveMotor.h"
#include "SteerMotor.h"

Timer t;

// Drive Motor Pins
DigitalOut br_ddir(p20);
PwmOut br_pwm(p21);
DigitalOut bl_ddir(p19);
PwmOut bl_pwm(p22);
DigitalOut fl_ddir(p18);
PwmOut fl_pwm(p23);
DigitalOut fr_ddir(p17);
PwmOut fr_pwm(p24);

// Steer Motor Pins
DigitalOut bl_sdir(p10);
DigitalOut bl_step(p9);
DigitalOut br_sdir(p12);
DigitalOut br_step(p11);
DigitalOut fl_sdir(p8);
DigitalOut fl_step(p7);
DigitalOut fr_sdir(p6);
DigitalOut fr_step(p5);


// Motors
SteerMotor bl_steer(bl_sdir, bl_step);
SteerMotor br_steer(br_sdir, br_step);
SteerMotor fl_steer(fl_sdir, fl_step);
SteerMotor fr_steer(fr_sdir, fr_step);
DriveMotor bl_drive(bl_ddir, bl_pwm);
DriveMotor br_drive(br_ddir, br_pwm);
DriveMotor fl_drive(fl_ddir, fl_pwm);
DriveMotor fr_drive(fr_ddir, fr_pwm);

PwmOut myled1(LED1);
PwmOut myled2(LED2);
PwmOut myled3(LED3);
PwmOut myled4(LED4);

uint32_t curr_time = 0; // In microseconds
 

int main() {   

    t.start();
    bl_steer.zero();
    bl_steer.zero();
    bl_steer.zero();
    bl_steer.zero();
    
    bl_drive.set_dir(1);
    br_drive.set_dir(1);
    fl_drive.set_dir(1);
    fr_drive.set_dir(1);
    
    fr_drive.set_multiplier(1.5);
    fl_drive.set_multiplier(1.15);
    bl_drive.set_multiplier(0.9);
    br_drive.set_multiplier(1.25);
    
    while(1){
        curr_time = t.read_us();
        bl_steer.update(curr_time);
        br_steer.update(curr_time);
        fl_steer.update(curr_time);
        fr_steer.update(curr_time);
        
        if (curr_time < 3000000){
            myled1 = 0;
            bl_drive.kill();
            br_drive.kill();
            fl_drive.kill();
            fr_drive.kill();
        }
        else if (curr_time < 8000000){
            myled1 = 1;
            bl_drive.set_speed(0.15);
            br_drive.set_speed(0.15);
            fl_drive.set_speed(0.15);
            fr_drive.set_speed(0.15);
        }
        else if (curr_time < 9000000){
            myled1 = 0;
            bl_drive.set_speed(0.0);
            br_drive.set_speed(0.0);
            fl_drive.set_speed(0.0);
            fr_drive.set_speed(0.0);
        }
        else if (curr_time < 14000000){
            myled1 = 1;
            bl_drive.set_dir(0);
            br_drive.set_dir(0);
            fl_drive.set_dir(0);
            fr_drive.set_dir(0);
            bl_drive.set_speed(0.15);
            br_drive.set_speed(0.15);
            fl_drive.set_speed(0.15);
            fr_drive.set_speed(0.15);
        }
        else if (curr_time < 15000000){
            myled1 = 0;
            bl_drive.kill();
            br_drive.kill();
            fl_drive.kill();
            fr_drive.kill();
        }
    }
}

 