#include "mbed.h"
#include "USBSerial.h"
#include "SoftPWM.h"

AnalogIn Y_Axis(P0_13);
AnalogIn X_Axis(P0_11);
AnalogIn Z_Axis(P0_15);

SoftPWM X_PWM(P0_9);  //P0_9
SoftPWM Y_PWM(P0_10); //P0_10
SoftPWM Z_PWM(P0_4); //P0_4

USBSerial serial;

#define MAX_LOGIC  10000   // Full-deflection Max Range
#define MIN_LOGIC  0       // Full-deflection Min Range

#define SWITCH_LEVELS  5  // # of possible levels
#define FWD            1
#define REV            0



//NOTE: Period MUST be defined 1st, them DC. 

float Period_Array [SWITCH_LEVELS] = {0.01, 0.1,  100.0, 0.1, 0.01};    // Period Corresponding to 10, 100, 0,  100, 10 Hz
uint16_t SpeedThreshold = MAX_LOGIC / SWITCH_LEVELS;

uint16_t X_Speed = 4;
uint16_t Y_Speed = 4;
uint16_t Z_Speed = 4;

bool X_Direction = FWD;
bool Y_Direction = FWD;
bool Z_Direction = FWD;

uint16_t tempX_Speed = 1;
uint16_t tempY_Speed = 1;
uint16_t tempZ_Speed = 1;


main()
{
    while(1)
  {
        wait_ms(5);

        X_Speed = (uint16_t) (X_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold /  SWITCH_LEVELS);
   
        Y_Speed = (uint16_t) (Y_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold /  SWITCH_LEVELS);
        
        Z_Speed = (uint16_t) (Z_Axis.read() * MAX_LOGIC) / (SpeedThreshold + SpeedThreshold /  SWITCH_LEVELS);
        
       if (X_Speed > 2) {X_Direction = FWD;}
       if (X_Speed < 2) {X_Direction = REV;}
       if (Y_Speed > 2) {Y_Direction = FWD;}
       if (Y_Speed < 2) {Y_Direction = REV;}
       if (Z_Speed > 2) {Z_Direction = FWD;}
       if (Z_Speed < 2) {Z_Direction = REV;}
        
        
        serial.printf(" X_Direction : %u",   X_Direction);
        serial.printf(" Y_Direction : %u \n",Y_Direction);
        serial.printf(" Z_Direction : %u \n",Z_Direction);
     
        if(tempX_Speed != X_Speed)
        {
            tempX_Speed = X_Speed;
            X_PWM.period(Period_Array[X_Speed]);   
            X_PWM.write(0.5);  
            
        serial.printf(" X_Period : %4.4f \n",Period_Array[X_Speed]);
        }
        
        if(tempY_Speed != Y_Speed)
        {
            tempY_Speed = Y_Speed;
            Y_PWM.period(Period_Array[Y_Speed]); 
            Y_PWM.write(0.5);    
        serial.printf(" Y_Period : %4.4f \n",Period_Array[Y_Speed]);
        }
        
        if(tempZ_Speed != Z_Speed)
        {
            tempZ_Speed = Z_Speed;
            Z_PWM.period(Period_Array[Z_Speed]); 
            Z_PWM.write(0.5);    
        serial.printf(" Z_Period : %4.4f \n",Period_Array[Z_Speed]);
        }
}
}