// ZenDrive Simulator firmware
// Copyright (c) 2021 - Valentin Boulanger
// Version : 1.0.0
//--------------------------------------------------------------------

#include "ZenDrive.h"

// Pins declaration
const int SPEEDS_PIN = A0;
const int HANDBRAKE_PIN = A5;
const int CLUTCH_PIN = A2;
const int BRAKE_PIN = A3;
const int ACCELERATOR_PIN = A4;
const int DIRECTION_PIN = A1;
const int LEFT_BLINKER_PIN = 2;
const int RIGHT_BLINKER_PIN = 3;
const int WARNING_PIN = 4;
const int CRUISE_PIN = 5;
const int CRUISE_UP_PIN = 6;
const int CRUISE_DOWN_PIN = 7;
const int STARTER_PIN = 8;
const int HORN_PIN = 9;
const int LIGHTS_ON_PIN = 10;
const int ROAD_LIGHTS_PIN = 11;
const int HEAD_LIGHTS_PIN = 12;
const int FOG_LIGHTS_PIN = 13;

void setup() { 
  // Initialize sensors
  pinMode(SPEEDS_PIN, INPUT_PULLUP);
  pinMode(HANDBRAKE_PIN, INPUT_PULLUP);
  pinMode(CLUTCH_PIN, INPUT_PULLUP);
  pinMode(BRAKE_PIN, INPUT_PULLUP);
  pinMode(ACCELERATOR_PIN, INPUT_PULLUP);
  pinMode(DIRECTION_PIN, INPUT_PULLUP);
  pinMode(LEFT_BLINKER_PIN, INPUT_PULLUP);
  pinMode(RIGHT_BLINKER_PIN, INPUT_PULLUP);
  pinMode(WARNING_PIN, INPUT_PULLUP);
  pinMode(CRUISE_PIN, INPUT_PULLUP);
  pinMode(CRUISE_UP_PIN, INPUT_PULLUP);
  pinMode(CRUISE_DOWN_PIN, INPUT_PULLUP);
  pinMode(STARTER_PIN, INPUT_PULLUP);
  pinMode(HORN_PIN, INPUT_PULLUP);
  pinMode(LIGHTS_ON_PIN, INPUT_PULLUP);
  pinMode(ROAD_LIGHTS_PIN, INPUT_PULLUP);
  pinMode(HEAD_LIGHTS_PIN, INPUT_PULLUP);
  pinMode(FOG_LIGHTS_PIN, INPUT_PULLUP);

  // Initialize ZenDrive Library
  ZenDrive.begin(true);

  //Button shared margin
  int btnMargin = 5;
}

void loop() {
  // Set refresh to false
  ZenDrive.gearshiftNeedRefresh = false;
  ZenDrive.pedalsNeedRefresh = false;
  ZenDrive.wheelNeedRefresh = false;
  
  /* Gearshift module */
  //Read speed states on the analog shared pin
  int value = analogRead(A0);
  if(value > 900) ZenDrive.switchSpeedR();
  else if(value > 800) ZenDrive.switchSpeed6();
  else if(value > 700) ZenDrive.switchSpeed5();
  else if(value > 600) ZenDrive.switchSpeed4();
  else if(value > 500) ZenDrive.switchSpeed3();
  else if(value > 400) ZenDrive.switchSpeed2();
  else if(value > 300) ZenDrive.switchSpeed1();
  else ZenDrive.switchNeutral();
  //Read handbrake
  ZenDrive.setHandbrake(map(analogRead(HANDBRAKE_PIN), 0, 1023, 0, 255));
  // Send gearshift states if there are changes
  if(ZenDrive.gearshiftNeedRefresh) ZenDrive.sendGearshiftStates();

  /* Pedals module */
  // Read clutch
  ZenDrive.setClutch(map(analogRead(CLUTCH_PIN), 0, 1023, 0, 255));
  // Read brake
  ZenDrive.setBrake(map(analogRead(BRAKE_PIN), 0, 1023, 0, 255));
  // Read accelerator
  ZenDrive.setAccelerator(map(analogRead(ACCELERATOR_PIN), 0, 1023, 0, 255));
  // Send pedals states if they are changes
  if(ZenDrive.pedalsNeedRefresh) ZenDrive.sendPedalsStates();

  /* Steering wheel module */  
  // Left blinker
  ZenDrive.setBlinkerLeft(digitalRead(LEFT_BLINKER_PIN));
  // Right blinker
  ZenDrive.setBlinkerRight(digitalRead(RIGHT_BLINKER_PIN));
  // Warning
  ZenDrive.setWarning(digitalRead(WARNING_PIN));
  // Lights
  if (digitalRead(LIGHTS_ON_PIN)) ZenDrive.switchLightsOn();
  else if(digitalRead(ROAD_LIGHTS_PIN)) ZenDrive.switchRoadLights();
  else ZenDrive.switchLightsOff();
  // Head lights
  ZenDrive.setHeadLights(digitalRead(HEAD_LIGHTS_PIN));
  // Fog lights
  ZenDrive.setFogLights(digitalRead(FOG_LIGHTS_PIN));
  // Starter
  ZenDrive.setStarter(digitalRead(STARTER_PIN));
  // Horn
  ZenDrive.setHorn(digitalRead(HORN_PIN));
  // Cruise on
  if(digitalRead(CRUISE_PIN)) ZenDrive.activeCruise();
  // Cruise up
  if(digitalRead(CRUISE_UP_PIN)) ZenDrive.increaseCruise();
  // Cruise down
  if(digitalRead(CRUISE_DOWN_PIN)) ZenDrive.decreaseCruise();  
  // Read direction
  ZenDrive.setDirection(map(analogRead(DIRECTION_PIN), 0, 1023, -127, 127));
  // Send steering wheel states if they are changes
  if(ZenDrive.wheelNeedRefresh) ZenDrive.sendWheelStates();

  // Read pin value
  /*int clutchValue = map(analogRead(clutchPin), 0, 1023, 0, 255);
  if(clutchValue < lastClutchState - 5 || clutchValue > lastClutchState + 5){
    //Serial.println(clutchValue);
    ZenDrive.setHandbrake(clutchValue);
    lastClutchState = clutchValue;
  }*/

  
  /*for (int index = 0; index < 4; index++)
  {
    int currentButtonState = !digitalRead(index + pinToButtonMap);
    if (currentButtonState != lastButtonState[index])
    {
      Joystick.setButton(index, currentButtonState);
      lastButtonState[index] = currentButtonState;
    }
  }*/
  delay(50);
}