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

#define CTRL_CNT 3   // number of 4 byte packages sent over bluetooth

Timer t;


// UART
Serial  pc(USBTX, USBRX);
RawSerial   bluetooth(p13,p14);


// 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);

// 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);

// 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);

// Modules
SwerveModule bl_module(&bl_steer, &bl_drive);
SwerveModule br_module(&br_steer, &br_drive);
SwerveModule fl_module(&fl_steer, &fl_drive);
SwerveModule fr_module(&fr_steer, &fr_drive);

//SwerveDrive swerve(&fl_module, &fr_module, &bl_module, &br_module);

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

uint32_t curr_time = 0; // In microseconds

//l_joy_x, l_joy_y, r_joy_x
float controller[CTRL_CNT] = {0};
 

int main() {   
    t.start();
    
    fr_drive.set_multiplier(1.5);
    fl_drive.set_multiplier(1.15);
    bl_drive.set_multiplier(0.9);
    br_drive.set_multiplier(1.25);
    
    bl_module.begin();
    br_module.begin();
    fl_module.begin();
    fr_module.begin();
    
//    swerve.begin();
    myled1 = 0;
    myled2 = 0;
    myled3 = 0;
    myled4 = 0;
    pc.baud(9600);
    bluetooth.baud(9600);
    
    uint32_t prev_time = 0;
    uint32_t interval = .1*1000000;
    
    int counter = 0;
    
    
    while(1) {
        curr_time = t.read_us();
        
        bl_module.update(curr_time);
        br_module.update(curr_time);
        fl_module.update(curr_time);
        fr_module.update(curr_time);
        
        if (curr_time < 3000000){
            bl_module.drive(0.1, 0.0);
            br_module.drive(0.1, 0.0);
            fl_module.drive(0.1, 0.0);
            fr_module.drive(0.1, 0.0);
        }
        else if (curr_time < 6000000){
            bl_module.drive(0.0, 90.0);
            br_module.drive(0.0, 90.0);
            fl_module.drive(0.0, 90.0);
            fr_module.drive(0.0, 90.0);
        }
        else if (curr_time < 9000000){
            bl_module.drive(0.1, 270.0);
            br_module.drive(0.1, 270.0);
            fl_module.drive(0.1, 270.0);
            fr_module.drive(0.1, 270.0);
        }
        else if (curr_time < 12000000){
            bl_module.kill();
            br_module.kill();
            fl_module.kill();
            fr_module.kill();
        }
        
        
//        swerve.update(curr_time);
//        swerve.drive(0.1f, 0.1f, 0.1f);
//        if ((curr_time - prev_time) > interval) {
//            myled4 = !myled4;
//            prev_time = curr_time;
//        }
//        if (pc.readable()) {
//            while (pc.getc() != 'a') {
//                }
//            for (int ind = 0; ind < CTRL_CNT; ind++) {
//                float f;
//                char b[4];
//                for (int byte_index = 0; byte_index < 4; byte_index++) {
//                    b[byte_index] = pc.getc(); 
//                }
//                memcpy(&f, &b, sizeof(f));
//                controller[ind] = f;
//            }
//        }

 
//        myled1 = abs(controller[0]);
//        myled2 = abs(controller[1]);
//        myled3 = abs(controller[2]);
    }
    
}

 