#include <Wire.h>
#include <PPMReader.h>
#include <drivers/PwmOut.h>

// Initialize a PPMReader on digital pin 3 with 6 expected channels.
byte interruptPin = 11;
byte channelAmount = 8;
PPMReader ppm(interruptPin, channelAmount);

#define PIN_TRIGGER 12
#define PIN_ECHO    13

int channel_ID_steering = 1;
int channel_ID_speed = 3;

const int SENSOR_MAX_RANGE = 300; // in cm
unsigned long duration;
unsigned int distance;

class Motor{
  int _pina;
  int _pinb;
  bool _first = true;
  float _prev = 0;
  mbed::PwmOut* _pwm = NULL;
public:
  void Setup(int a, int b)
  {
    _pina = a;
    _pinb = b;
    pinMode(_pina, OUTPUT);
    pinMode(_pinb, OUTPUT);
  }
  void SetPower(float pow)
  {
    if(_first){
      _first = false;
      _prev = pow;
    }
      if(_pwm != NULL)
      {
        delete _pwm;
        _pwm = NULL;
      }
    if(pow < 0)
    {
      if(_pwm == NULL)
      {
        _pwm = new mbed::PwmOut(digitalPinToPinName(_pinb));
        _pwm->period_ms(2); //500Hz
      }
      digitalWrite(_pina,0);
      
      _pwm->write(-pow);      
    }    
    else
    {
      if(_pwm == NULL)
      {
        _pwm = new mbed::PwmOut(digitalPinToPinName(_pina));
        _pwm->period_ms(2); //500Hz
      }
      digitalWrite(_pinb,0);
      
      _pwm->write(pow);
    }    
  }
}; // ; am Ende einer Klasse

Motor motor_fl;
Motor motor_fr;
Motor motor_rl;
Motor motor_rr;

void setup() {
  Serial.begin(9600);
  
  motor_fl.Setup(2, 3);
  motor_fr.Setup(9, 8);
  motor_rl.Setup(5, 4);
  motor_rr.Setup(6, 7);
  Wire.begin(); // join i2c bus (address optional for master)
  pinMode(A4, INPUT_PULLUP);
  pinMode(A5, INPUT_PULLUP);
}
 
void loop() {
    int steering_value_raw = ppm.latestValidChannelValue(channel_ID_steering, 0);
    int speed_value_raw = ppm.latestValidChannelValue(channel_ID_speed, 0);

    float steering_value = (steering_value_raw == 0)? 1 : (1/500.0f) * steering_value_raw - 3; // == Bedingung, danach wenn True: wenn False 
    float speed_value = (speed_value_raw == 0)? 1 : (1/500.0f) * speed_value_raw - 3;

    if (steering_value < -1)
      steering_value = -1;
    else if (steering_value > 1)
      steering_value = 1;


    float motor_right = (-steering_value + speed_value); 
    float motor_left = (steering_value + speed_value);
    float factor = max (abs(motor_right), abs(motor_left));
    if (factor > 1) {
      motor_right = motor_right / factor;
      motor_left = motor_left / factor;
    }
    

    delay(20);
  
  motor_rr.SetPower(motor_right);
  motor_rl.SetPower(motor_left);
  motor_fr.SetPower(motor_right);
  motor_fl.SetPower(motor_left);

  Serial.println(String(motor_left) + "\t" + String(motor_right));
  //while(true);
}