#include "InterCom.h"

SimpleComand cmd;

#define max_v_in_divisor  24.0
#define max_value_adc_resolution 4095.0

const uint8_t en_pin = 2, pwm_pin = 4, dir_pin[2] = {16,17};
const uint8_t pin_adc = 34;

bool en_m = false;


float Vd = 0, Vm = 0;

void motor_en(){
  en_m = !en_m;
  digitalWrite(en_pin,en_m);
  Serial.print("Motors: ");
  // escribimops el estado
  if(en_m){
    Serial.println("Activated");
  }
  else{
    Serial.println("Desactivated");
  }
}

void motor_speed(int pwm){
  if(pwm < 0){
    digitalWrite(dir_pin[0],LOW);
    digitalWrite(dir_pin[1],HIGH);
  }
  else if (pwm > 0){
    digitalWrite(dir_pin[1],LOW);
    digitalWrite(dir_pin[0],HIGH);
  }
  else{
    digitalWrite(dir_pin[1],LOW); //Break
    digitalWrite(dir_pin[0],LOW);
  }
  analogWrite(pwm_pin, abs(pwm));
}

void update_vm(){
  Vm = (float)analogRead(pin_adc) * (max_v_in_divisor/max_value_adc_resolution);
}

int update_pwm(){
  return (int)(Vd/Vm * (255.0));
}

void setup() {
  pinMode(en_pin,OUTPUT);
  pinMode(pwm_pin,OUTPUT);
  pinMode(dir_pin[0],OUTPUT);
  pinMode(dir_pin[1],OUTPUT);

  pinMode(pin_adc,INPUT);

  cmd.begin(115200);
  cmd.enable_echo(true);

  cmd.addComand("en",motor_en);
  cmd.addComand("v",&Vd);
}

void loop() {
  cmd.lisent();

  update_vm();
  motor_speed(update_pwm());
}