/* ***DIRECTION.ino***
   author: Duhamel Loïc
   copyright: Copyright (C) 2020 Duhamel Loïc
   license: Public Domain
   version: 0.3.0 (created 14I2021, last modified 22I2021)
   aim: distribution of command voltage (Setpoint, or SP) between Left side and Right side controlers for
        L7-type vehicles, based on data from a 3-axis joystick
*/


/*
  check int/byte etc...

  à verifier en fonctionnement:
  bool braking
  constrain
  epsilon
  gerer map en X/R differement
  sensibilité CTLR dépend de 3pos selection ?
  actuation non linéaire
  délai/moyennage avec RC low pass, pour plus de sensibilité
*/

/* BUG FIXES
 *  Clear Cache
   constrain()
   check si peut dépasser valeurs en PWM
   check si grave que commande trop grande vers CTLR
   check si grave que PWM et pas analogique: non
*/

/* AMELIORATIONS
   ajuster plage de commande
   quand tout droit, deux roues même rpm ?? asservissement en rpm (fin, ie pas 1 par tour...) ou en couple (??)
   integrer freins
   turn while braking ?
   brake while turning: freinage progressif ???? ajustement regen en live ?
   marche arrière
   faire tourner les deux cotés dans deux sens differents ? dans V0, tourner en accélérant
   hysteresis ?
   klaxon
   feux quand frein
*/

// input pins
const byte pinPressIn = 2; // digital
const byte pinXfromJS = A0; // analogic in
const byte pinRfromJS = A1; // analogic in

// output pins
const byte pinPressOut = 3; // digital (with PWM if needed)
const byte pinSPtoCTLRarg = 5; // PWM digital, add low-pass RC filter?
const byte pinSPtoCTLRard = 6; // PWM digital, add low-pass RC filter?
const byte pinBrakeCTLRarg = 7; // digital
const byte pinBrakeCTLRard = 8; // digital
const byte pinSPtoCTLRavg = 9; // PWM digital, add low-pass RC filter?
const byte pinSPtoCTLRavd = 10; // PWM digital, add low-pass RC filter?
const byte pinBrakeCTLRavg = 12; // digital
const byte pinBrakeCTLRavd = 13; // digital

// data to retreive from "direction-test", depending on JS
const int XminfromJS = 0;
const int XmiddleJS = 520;
const int XmaxfromJS = 1023;
const int RminfromJS = 0;
const int RmiddleJS = 520;
const int RmaxfromJS = 1023;

//data to update for each CTLR, depends just on the setpoint range that actually actuates the CTLR.
//may depend on the speed selector position.
const int XtoCTLRmin = 45;
const int XtoCTLRmax = 80;

// parameters
const float epsilon = 8;

// global variables
bool honkOn = false;
bool braking = false;


void setup() {
  Serial.begin(9600);

  pinMode(pinPressIn, INPUT);
  pinMode(pinXfromJS, INPUT);
  pinMode(pinRfromJS, INPUT);

  pinMode(pinPressOut, OUTPUT);
  pinMode(pinSPtoCTLRarg, OUTPUT);
  pinMode(pinSPtoCTLRard, OUTPUT);
  pinMode(pinSPtoCTLRavg, OUTPUT);
  pinMode(pinSPtoCTLRavd, OUTPUT);
  pinMode(pinBrakeCTLRarg, OUTPUT);
  pinMode(pinBrakeCTLRard, OUTPUT);
  pinMode(pinBrakeCTLRavg, OUTPUT);
  pinMode(pinBrakeCTLRavd, OUTPUT);

  Serial.println("initiated");
}

void loop() {

  // reading JoyStick position
  float XfromJS = analogRead(pinXfromJS);  // int normalement?
  float RfromJS = analogRead(pinRfromJS);  // idem

  // THROTTLE ON
  if (XfromJS > XmiddleJS + epsilon && XfromJS <= XmaxfromJS) {

    if (braking) {
      braking = false;
      digitalWrite(pinBrakeCTLRarg, HIGH);
      digitalWrite(pinBrakeCTLRard, HIGH);
      digitalWrite(pinBrakeCTLRavg, HIGH);
      digitalWrite(pinBrakeCTLRavd, HIGH);
    }

    float XtoCalc = map(XfromJS - XmiddleJS, epsilon, XmaxfromJS - XmiddleJS, XtoCTLRmin, XtoCTLRmax);

    // TURNING RIGHT
    if (RfromJS > RmiddleJS + 3 * epsilon && RfromJS <= RmaxfromJS) {
      float RtoCalc = map(RfromJS - RmiddleJS, epsilon, RmaxfromJS - RmiddleJS, XtoCTLRmin, XtoCTLRmax);
      int INTtoCalcD = int(max(0, XtoCalc - RtoCalc)); // right slower
      int INTtoCalcG = int(min(255, XtoCalc + RtoCalc)); // left faster
      analogWrite(pinSPtoCTLRavd, INTtoCalcD);
      analogWrite(pinSPtoCTLRard, INTtoCalcD);
      analogWrite(pinSPtoCTLRavg, INTtoCalcG);
      analogWrite(pinSPtoCTLRarg, INTtoCalcG);
      // min useless with map to 100, but better to constrain
      Serial.print(INTtoCalcD);
      Serial.print(" ");
      Serial.print(INTtoCalcG);
    }
    // TURNING LEFT
    if (RfromJS < RmiddleJS - 3 * epsilon && RfromJS >= RminfromJS) {
      float RtoCalc = map(RmiddleJS - RfromJS, epsilon, RmiddleJS - RminfromJS, XtoCTLRmin, XtoCTLRmax);
      int INTtoCalcD = int(min(255, XtoCalc + RtoCalc)); // right faster
      int INTtoCalcG = int(max(0, XtoCalc - RtoCalc)); // left slower
      analogWrite(pinSPtoCTLRavd, INTtoCalcD);
      analogWrite(pinSPtoCTLRard, INTtoCalcD);
      analogWrite(pinSPtoCTLRavg, INTtoCalcG);
      analogWrite(pinSPtoCTLRarg, INTtoCalcG);

      Serial.print(INTtoCalcD);
      Serial.print(" ");
      Serial.print(INTtoCalcG);
    }

    //STRAIGHT
    if ( abs(RfromJS - RmiddleJS) < 3 * epsilon ) {
      analogWrite(pinSPtoCTLRavd, XtoCalc); // right same
      analogWrite(pinSPtoCTLRard, XtoCalc); // right same
      analogWrite(pinSPtoCTLRavg, XtoCalc);  // left same
      analogWrite(pinSPtoCTLRarg, XtoCalc);  // left same

      Serial.println(XtoCalc);
    }

    else {
      analogWrite(pinSPtoCTLRavd, 0);
      analogWrite(pinSPtoCTLRard, 0);
      analogWrite(pinSPtoCTLRavg, 0);
      analogWrite(pinSPtoCTLRarg, 0);

      Serial.println(pinSPtoCTLRavd, 0);
      Serial.println(pinSPtoCTLRard, 0);
      Serial.println(pinSPtoCTLRavg, 0);
      Serial.println(pinSPtoCTLRarg, 0);
    }
  }

  // BRAKE ON
  if (XfromJS < XmiddleJS - 5 * epsilon) {
    if (!braking) {
      braking = true;
      digitalWrite(pinBrakeCTLRavd, LOW); // to be changed if need to control direction while braking
      digitalWrite(pinBrakeCTLRavg, LOW); // to be changed if need to control direction while braking
      digitalWrite(pinBrakeCTLRard, LOW); // to be changed if need to control direction while braking
      digitalWrite(pinBrakeCTLRarg, LOW); // to be changed if need to control direction while braking
    }
    //digitalWrite pin feux AR high/ ajouter relai vers 12v feux AR en série
  }

  //Press
  //if (digitalRead(pinPressIn)==LOW) {
  //  Serial.println("Klaxon");



  /* TO USE CENTER BUTTON
    honkOn = digitalRead(pinPressIn);
    if (honkOn) {
    honking(); //s'éteint quand ? si temps, attention à pas bloquer la direction
    arduino parallelization ?
    }
  */
}
/* TO BE USED WITH "TO USE CENTER BUTTON"
  void honking(){

  }
*/