/*
  ZenDrive.cpp

  Copyright (c) 2021, Valentin Boulanger

  This library is a part of the ZenDrive simulator firmware.
  Version : 1.0.0
*/

#include "ZenDrive.h"

#if defined(_USING_HID)

#define GEARSHIFT_REPORT_ID 0x03
#define GEARSHIFT_STATE_SIZE 2

#define PEDALS_REPORT_ID 0x04
#define PEDALS_STATE_SIZE 3

#define WHEEL_REPORT_ID 0x05
#define WHEEL_STATE_SIZE 3

static const uint8_t _hidReportDescriptor[] PROGMEM = {
  
	 0x05, 0x01,                    // USAGE_PAGE (Generic Desktop)
    0x09, 0x04,                    // USAGE (Joystick)
    0xa1, 0x01,                    // COLLECTION (Application)
    0x85, 0x03,                    //   REPORT_ID (3)
    0x05, 0x09,                    //   USAGE_PAGE (Button)
    0x19, 0x01,                    //   USAGE_MINIMUM (Button 1)
    0x29, 0x08,                    //   USAGE_MAXIMUM (Button 8)
    0x15, 0x00,                    //   LOGICAL_MINIMUM (0)
    0x25, 0x01,                    //   LOGICAL_MAXIMUM (1)
    0x95, 0x08,                    //   REPORT_COUNT (8)
    0x75, 0x01,                    //   REPORT_SIZE (1)
    0x81, 0x02,                    //   INPUT (Data,Var,Abs)
    0x05, 0x01,                    //   USAGE_PAGE (Generic Desktop)
    0x15, 0x00,                    //   LOGICAL_MINIMUM (0)
    0x26, 0xff, 0x00,              //   LOGICAL_MAXIMUM (255)
    0xa1, 0x00,                    //   COLLECTION (Physical)
    0x09, 0x32,                    //     USAGE (Z)
    0x75, 0x08,                    //     REPORT_SIZE (8)
    0x95, 0x01,                    //     REPORT_COUNT (1)
    0x81, 0x02,                    //     INPUT (Data,Var,Abs)
    0xc0,                          //   END_COLLECTION
    0xc0,                          // END_COLLECTION
    0x05, 0x01,                    // USAGE_PAGE (Generic Desktop)
    0x09, 0x04,                    // USAGE (Joystick)
    0xa1, 0x01,                    // COLLECTION (Application)
    0x85, 0x04,                    //   REPORT_ID (4)
    0x05, 0x01,                    //   USAGE_PAGE (Generic Desktop)
    0x15, 0x00,                    //   LOGICAL_MINIMUM (0)
    0x26, 0xff, 0x00,              //   LOGICAL_MAXIMUM (255)
    0xa1, 0x00,                    //   COLLECTION (Physical)
    0x09, 0x33,                    //     USAGE (Rx)
    0x09, 0x34,                    //     USAGE (Ry)
    0x09, 0x35,                    //     USAGE (Rz)
    0x75, 0x08,                    //     REPORT_SIZE (8)
    0x95, 0x03,                    //     REPORT_COUNT (3)
    0x81, 0x02,                    //     INPUT (Data,Var,Abs)
    0xc0,                          //   END_COLLECTION
    0xc0,                          // END_COLLECTION
    0x05, 0x01,                    // USAGE_PAGE (Generic Desktop)
    0x09, 0x04,                    // USAGE (Joystick)
    0xa1, 0x01,                    // COLLECTION (Application)
    0x85, 0x05,                    //   REPORT_ID (5)
    0x05, 0x09,                    //   USAGE_PAGE (Button)
    0x19, 0x09,                    //   USAGE_MINIMUM (Button 9)
    0x29, 0x13,                    //   USAGE_MAXIMUM (Button 19)
    0x15, 0x00,                    //   LOGICAL_MINIMUM (0)
    0x25, 0x01,                    //   LOGICAL_MAXIMUM (1)
    0x75, 0x01,                    //   REPORT_SIZE (1)
    0x95, 0x0b,                    //   REPORT_COUNT (11)
    0x81, 0x02,                    //   INPUT (Data,Var,Abs)
    0x95, 0x01,                    //   REPORT_COUNT (1)
    0x75, 0x05,                    //   REPORT_SIZE (5)
    0x81, 0x03,                    //   INPUT (Cnst,Var,Abs)
    0x05, 0x02,                    //   USAGE_PAGE (Simulation Controls)
    0x15, 0x81,                    //   LOGICAL_MINIMUM (-127)
    0x25, 0x7f,                    //   LOGICAL_MAXIMUM (127)
    0xa1, 0x00,                    //   COLLECTION (Physical)
    0x09, 0xc8,                    //     USAGE (Steering)
    0x75, 0x08,                    //     REPORT_SIZE (8)
    0x95, 0x01,                    //     REPORT_COUNT (1)
    0x81, 0x02,                    //     INPUT (Data,Var,Abs)
    0xc0,                          //   END_COLLECTION
    0xc0                           // END_COLLECTION
};

ZenDrive_::ZenDrive_()
{
	// Setup HID report structure
	static HIDSubDescriptor node(_hidReportDescriptor, sizeof(_hidReportDescriptor));
	HID().AppendDescriptor(&node);
}

void ZenDrive_::begin()
{
  // Sending initial states
  sendGearshiftStates();
  sendPedalsStates();
  sendWheelStates();
}

void ZenDrive_::end()
{
}

// Gearshift module
void ZenDrive_::switchNeutral()
{
  if(currentSpeed != 0){
    bitSet(speeds, 0);
    bitClear(speeds, 1);
    bitClear(speeds, 2);
    bitClear(speeds, 3);
    bitClear(speeds, 4);
    bitClear(speeds, 5);
    bitClear(speeds, 6);
    bitClear(speeds, 7);
    currentSpeed = 0;
    gearshiftNeedRefresh = true;
  }
}
void ZenDrive_::switchSpeed1()
{
  if(currentSpeed != 1){
    bitClear(speeds, 0);
    bitSet(speeds, 1);
    bitClear(speeds, 2);
    bitClear(speeds, 3);
    bitClear(speeds, 4);
    bitClear(speeds, 5);
    bitClear(speeds, 6);
    bitClear(speeds, 7);
    currentSpeed = 1;
    gearshiftNeedRefresh = true;
  }
}
void ZenDrive_::switchSpeed2()
{
  if(currentSpeed != 2)
  {
    bitClear(speeds, 0);
    bitClear(speeds, 1);
    bitSet(speeds, 2);
    bitClear(speeds, 3);
    bitClear(speeds, 4);
    bitClear(speeds, 5);
    bitClear(speeds, 6);
    bitClear(speeds, 7);
    currentSpeed = 2;
    gearshiftNeedRefresh = true;
  }
}
void ZenDrive_::switchSpeed3()
{
  if(currentSpeed != 3)
  {
    bitClear(speeds, 0);
    bitClear(speeds, 1);
    bitClear(speeds, 2);
    bitSet(speeds, 3);
    bitClear(speeds, 4);
    bitClear(speeds, 5);
    bitClear(speeds, 6);
    bitClear(speeds, 7);
    currentSpeed = 3;
    gearshiftNeedRefresh = true;
  }
}
void ZenDrive_::switchSpeed4()
{
  if(currentSpeed != 4)
  {
    bitClear(speeds, 0);
    bitClear(speeds, 1);
    bitClear(speeds, 2);
    bitClear(speeds, 3);
    bitSet(speeds, 4);
    bitClear(speeds, 5);
    bitClear(speeds, 6);
    bitClear(speeds, 7);
    currentSpeed = 4;
    gearshiftNeedRefresh = true;
  }
}
void ZenDrive_::switchSpeed5()
{
  if(currentSpeed != 5)
  {
    bitClear(speeds, 0);
    bitClear(speeds, 1);
    bitClear(speeds, 2);
    bitClear(speeds, 3);
    bitClear(speeds, 4);
    bitSet(speeds, 5);
    bitClear(speeds, 6);
    bitClear(speeds, 7);
    currentSpeed = 5;
    gearshiftNeedRefresh = true;
  }
}
void ZenDrive_::switchSpeed6()
{
  if(currentSpeed != 6)
  {
    bitClear(speeds, 0);
    bitClear(speeds, 1);
    bitClear(speeds, 2);
    bitClear(speeds, 3);
    bitClear(speeds, 4);
    bitClear(speeds, 5);
    bitSet(speeds, 6);
    bitClear(speeds, 7);
    currentSpeed = 6;
    gearshiftNeedRefresh = true;
  }
}
void ZenDrive_::switchSpeedR()
{
  if(currentSpeed != 7)
  {
    bitClear(speeds, 0);
    bitClear(speeds, 1);
    bitClear(speeds, 2);
    bitClear(speeds, 3);
    bitClear(speeds, 4);
    bitClear(speeds, 5);
    bitClear(speeds, 6);
    bitSet(speeds, 7);
    currentSpeed = 7;
    gearshiftNeedRefresh = true;
  }
}

void ZenDrive_::setHandbrake(int8_t value)
{
  if(value < handbrake - 5 || value > handbrake + 5)
  {
    handbrake = value;
    gearshiftNeedRefresh = true;
  }
}

// Pedals module
void ZenDrive_::setClutch(int8_t value)
{
  if(value < clutch - 5 || value > clutch + 5)
  {
    clutch = value;
    pedalsNeedRefresh = true;
  }
}

void ZenDrive_::setBrake(int8_t value)
{
  if(value < brake - 5 || value > brake + 5)
  {
    brake = value;
    pedalsNeedRefresh = true;
  }
  
}

void ZenDrive_::setAccelerator(int8_t value)
{
  if(value < accelerator - 5 || value > accelerator + 5)
  {
    accelerator = value;
    pedalsNeedRefresh = true;
  }
}

// Steering wheel module
void ZenDrive_::setBlinkerLeft(bool value)
{
  if(value != isBlinkerLeft)
  {
    bitSet(wheelCommands, 0);
    sendWheelStates();
    delay(50);
    bitClear(wheelCommands, 0);
    sendWheelStates();
    isBlinkerLeft = !isBlinkerLeft;
  }  
}

void ZenDrive_::setBlinkerRight(bool value)
{
  if(value != isBlinkerRight)
  {
    bitSet(wheelCommands, 1);
    sendWheelStates();
    delay(50);
    bitClear(wheelCommands, 1);
    sendWheelStates();
    isBlinkerRight = !isBlinkerRight;  
  }
}

void ZenDrive_::setWarning(bool value)
{
  if(value != isWarning)
  {
    bitSet(wheelCommands, 2);
    sendWheelStates();
    delay(50);
    bitClear(wheelCommands, 2);
    sendWheelStates();
    isWarning = !isWarning;  
  }
}

void ZenDrive_::switchLightsOn()
{
  int steps = 0;
  if(isRoadLights) steps = 2;
  else if(!isLights) steps = 1;
  if(steps != 0){
    for(int i = 0; i < steps; i++){
      bitSet(wheelCommands, 3);
      sendWheelStates();
      delay(50);
      bitClear(wheelCommands, 3);
      sendWheelStates();
      delay(50);
      isLights = true;
      isRoadLights = false; 
    }
  }
}

void ZenDrive_::switchRoadLights()
{
  int steps = 0;
  if(isLights) steps = 1;
  else if(!isRoadLights) steps = 2;
  if(steps != 0){
    for(int i = 0; i < steps; i++){
      bitSet(wheelCommands, 3);
      sendWheelStates();
      delay(50);
      bitClear(wheelCommands, 3);
      sendWheelStates();
      delay(50);
      isLights = false;
      isRoadLights = true; 
    }
  }
}

void ZenDrive_::switchLightsOff()
{
  int steps = 0;
  if(isLights) steps = 2;
  else if(isRoadLights) steps = 1;
  if(steps != 0){
    for(int i = 0; i < steps; i++){
      bitSet(wheelCommands, 3);
      sendWheelStates();
      delay(50);
      bitClear(wheelCommands, 3);
      sendWheelStates();
      delay(50);
      isLights = false;
      isRoadLights = false; 
    }
  }
}

void ZenDrive_::setHeadLights(bool value)
{
  if(value != isHeadLights)
  {
    bitSet(wheelCommands, 4);
    sendWheelStates();
    delay(50);
    bitClear(wheelCommands, 4);
    sendWheelStates();
    isHeadLights = !isHeadLights;  
  }
}

void ZenDrive_::setFogLights(bool value)
{
  if(value != isFogLights)
  {
    bitSet(wheelCommands, 5);
    sendWheelStates();
    delay(50);
    bitClear(wheelCommands, 5);
    sendWheelStates();
    isFogLights = !isFogLights;  
  }
}

void ZenDrive_::setStarter(bool value)
{
  if(value != isStarter)
  {
    bitSet(wheelCommands, 6);
    sendWheelStates();
    delay(50);
    bitClear(wheelCommands, 6);
    sendWheelStates();
    isStarter = !isStarter;  
  }
}

void ZenDrive_::setHorn(bool value)
{
  if(value != isHorn)
  {
    if(value) bitSet(wheelCommands, 7);
    else bitClear(wheelCommands, 7);
    isHorn = !isHorn;
    wheelNeedRefresh = true;
  }
}

void ZenDrive_::activeCruise()
{
  bitSet(wheelCommands, 8);
  sendWheelStates();
  delay(50);
  bitClear(wheelCommands, 8);
  sendWheelStates();
  wheelNeedRefresh = false;
}

void ZenDrive_::increaseCruise()
{
  bitSet(wheelCommands, 9);
  sendWheelStates();
  delay(50);
  bitClear(wheelCommands, 9);
  sendWheelStates();
  wheelNeedRefresh = false;
}

void ZenDrive_::decreaseCruise()
{
  bitSet(wheelCommands, 10);
  sendWheelStates();
  delay(50);
  bitClear(wheelCommands, 10);
  sendWheelStates();
  wheelNeedRefresh = false;
}

void ZenDrive_::setDirection(uint8_t value)
{
  if(value < steering - 5 || value > steering + 5)
  {
    steering = value;
    wheelNeedRefresh = true;
  }
}

// Send states to the PC
void ZenDrive_::sendGearshiftStates()
{
  // Gearshift module
  uint8_t dataGearshift[GEARSHIFT_STATE_SIZE];
  uint8_t speedTmp = speeds;
  dataGearshift[0] = speedTmp & 0xFF;
  dataGearshift[1] = handbrake;

  HID().SendReport(GEARSHIFT_REPORT_ID, dataGearshift, GEARSHIFT_STATE_SIZE);
}

void ZenDrive_::sendPedalsStates()
{
  // Pedals module
  uint8_t dataPedals[PEDALS_STATE_SIZE];
  dataPedals[0] = clutch;
  dataPedals[1] = brake;
  dataPedals[2] = accelerator;

  HID().SendReport(PEDALS_REPORT_ID, dataPedals, PEDALS_STATE_SIZE);
}

void ZenDrive_::sendWheelStates()
{
  // Steering wheel module
  uint8_t dataWheel[WHEEL_STATE_SIZE];
  uint16_t commandTmp = wheelCommands;

  // Split 16 bit button-state into 2 bytes
  dataWheel[0] = commandTmp & 0xFF;   
  commandTmp >>= 8;
  dataWheel[1] = commandTmp & 0xFF;
  dataWheel[2] = steering;

  HID().SendReport(WHEEL_REPORT_ID, dataWheel, WHEEL_STATE_SIZE);

  /*data[7] = xAxis + 127;
  data[8] = yAxis + 127;
  data[9] = zAxis + 127;

  data[10] = (xAxisRotation % 360) * 0.708;
  data[11] = (yAxisRotation % 360) * 0.708;
  data[12] = (zAxisRotation % 360) * 0.708;*/
}

ZenDrive_ ZenDrive;

#endif