#include "Dynamixel_Servo.h"

#define HALF_DUPLEX_DIRECTION_PIN 6
#define SERVO_TIMEOUT 200 //milliseconds

#define FORWARD_DIRECTION_PIN 7
#define REVERSE_DIRECTION_PIN 8

#define STEERING_INPUT 14
#define THROTTLE_INPUT 15

#define THROTTLE_A 3
#define THROTTLE_B 5

// #define LOWEST_PULSE_THROTTLE 17850
// #define HIGHEST_PULSE_THROTTLE 18700

#define LOWEST_PULSE_THROTTLE 17950
#define HIGHEST_PULSE_THROTTLE 18900

#define LOWEST_PULSE_STEERING 17780
#define HIGHEST_PULSE_STEERING 18790


static float CHAN_A_MIN_VOLTAGE = 1.37 - 0.11;
static float CHAN_A_MAX_VOLTAGE = (3.82 - 0.28)*0.50;  // NOTE: Go lower bc we don't wanna go supersonic during the demo
static float CHAN_B_MIN_VOLTAGE = 0.68 - 0.15;
static float CHAN_B_MAX_VOLTAGE = (3.18 - 0.37)*0.50;  // NOTE: Go lower bc we don't wanna go supersonic during the demo

int32_t throttle_in;
int32_t steering_in;
int32_t throttle_in_mapped;
int32_t steering_in_mapped;

int throttle_speed;
int steering_amount;

int mapped_throttle_chan_a;
int mapped_throttle_chan_b;

struct channel_outputs {
    double chan_a;
    double chan_b;
};

float mapping(float in, float in_min, float in_max, float out_min, float out_max){
  float mapped = (((in - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min;
  return mapped;
}


void setup() {
  pinMode(STEERING_INPUT, INPUT_PULLUP);
  pinMode(THROTTLE_INPUT, INPUT_PULLUP);
  pinMode(THROTTLE_A, OUTPUT);
  pinMode(THROTTLE_B, OUTPUT);
  pinMode(FORWARD_DIRECTION_PIN, OUTPUT);
  pinMode(REVERSE_DIRECTION_PIN, OUTPUT);
  digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
  digitalWrite(REVERSE_DIRECTION_PIN, HIGH);

  servo_init(&Serial1, HALF_DUPLEX_DIRECTION_PIN, SERVO_DEFAULT_BAUD);

  Serial.begin(9600);
  delay(2000);
  Serial.println("Starting...");
}


void steering_zero() {
  servo_error_t error;
  int steering_angle_deg = 0;
  error = servo_set(SERVO_DEFAULT_ID, SERVO_REGISTER_GOAL_ANGLE, steering_angle_deg, SERVO_TIMEOUT);
  Serial.print(" STEERING ZERO ");
}

void set_steering(float angle_deg){
  servo_error_t error;
  float angle_rad = angle_deg * 1000. / 57296.;
  error = servo_set(SERVO_DEFAULT_ID, SERVO_REGISTER_GOAL_ANGLE, angle_rad, SERVO_TIMEOUT);
  if (! (error & SERVO_NO_ERROR)){
    Serial.print("SERVO ERROR: ");
    Serial.println(error);
  }
}


void steering(int32_t steering_pwm) {
  if (steering_pwm < 17000) {
    steering_zero();
  } else {
    steering_in_mapped = map(steering_pwm, LOWEST_PULSE_STEERING, HIGHEST_PULSE_STEERING, 0, 1023);
    set_steering(steering_in_mapped);
  }
}


void loop() {
  steering_in = pulseIn(STEERING_INPUT, LOW);
  Serial.print(" SPIN: ");
  Serial.print(steering_in);
  // steering(steering_in);
  set_steering(512);
  delay(500);
  set_steering(0);
  delay(500);
  Serial.println();
}