#include <Dynamixel2Arduino.h>
#include <Adafruit_MCP4728.h>
#include <Wire.h>

#define DXL_SERIAL   Serial1
#define DEBUG_SERIAL Serial
const uint8_t DXL_DIR_PIN = 4; // DYNAMIXEL Shield DIR PIN

#define MAX_V 5.2
#define SPEED_MULTIPLIER 0.8

const uint8_t DXL_ID = 1;
const float DXL_PROTOCOL_VERSION = 1.0;

Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN);
using namespace ControlTableItem;

Adafruit_MCP4728 mcp;

long zero_offset = 0.0;

//TOP THROT: 18792 | BOT THROT: 17991 | TOP STEER: 19092 | BOT STEER: 17691


#define FORWARD_DIRECTION_PIN 7
#define REVERSE_DIRECTION_PIN 8

#define STEERING_INPUT 14
#define THROTTLE_INPUT 15

#define TRIM_RIGHT 16
#define TRIM_LEFT 10 //TODO: change pins
#define TRIM_DELTA 100 //TODO: tune this


#define LOWEST_PULSE_THROTTLE 17970
#define HIGHEST_PULSE_THROTTLE 18800

// #define LOWEST_PULSE_STEERING 17650
// #define HIGHEST_PULSE_STEERING 19150

#define LOWEST_PULSE_STEERING 17600
#define HIGHEST_PULSE_STEERING 19200


static float CHAN_A_MIN_VOLTAGE = 1.37 + 0.0;
static float CHAN_A_MAX_VOLTAGE = (3.82 + 0.0) * SPEED_MULTIPLIER;  // NOTE: Go lower bc we don't wanna go supersonic during the demo
static float CHAN_B_MIN_VOLTAGE = 0.68 + 0.0;
static float CHAN_B_MAX_VOLTAGE = (3.18 + 0.0) * SPEED_MULTIPLIER;  // 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;
long 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 init_servo(){
  dxl.begin(57600);
  dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION);
  dxl.ping(DXL_ID);
  dxl.torqueOff(DXL_ID);
  dxl.setOperatingMode(DXL_ID, OP_EXTENDED_POSITION);
  dxl.writeControlTableItem(RESOLUTION_DIVIDER, DXL_ID, 1);
  dxl.torqueOn(DXL_ID);
  dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID, 0);
  delay(500);
}

int convert_to_digital(float v){
  int v_out;
  v_out = (v/5.0) * 4095;
  return v_out;
}
int minmax(int v){
  if (v > 4095) {v = 4095;}
  if (v < 0) {v = 0;}
  return v;
}

float low_voltage_compensator(float v){
  float v_offset = 5.0 - MAX_V;
  float v_proportional = v/5.0;
  float adjusted_v = v + (v_proportional * v_offset);
//  return v;
  return adjusted_v;
}

int process_voltage(float v){
  float v_adj = low_voltage_compensator(v);
  int v_int = convert_to_digital(v_adj);
  v_int = minmax(v_int);
  return v_int;
}

void set_voltage_a(float v){
  int v_int = process_voltage(v);
  mcp.setChannelValue(MCP4728_CHANNEL_A, v_int);
}
void set_voltage_b(float v){
  int v_int = process_voltage(v);
  mcp.setChannelValue(MCP4728_CHANNEL_B, v_int);
}

void increment_trim(int amount){
  zero_offset = zero_offset + amount;
}

void decrement_trim(int amount){
  zero_offset = zero_offset - amount;
}

void stop_throttle() {
  set_voltage_a(CHAN_A_MIN_VOLTAGE);
  set_voltage_b(CHAN_B_MIN_VOLTAGE);
  Serial.print(" THROTTLE STOPPED ");
}

void steering_zero() {
  dxl.setGoalPosition(DXL_ID, 0+zero_offset);
}

void set_throttle(int speed){
  channel_outputs pwm_out = pwm_to_outputs(speed);
  set_voltage_a(pwm_out.chan_a);
  set_voltage_b(pwm_out.chan_b);
}

void go_forward(int speed){
    set_forward();
    set_throttle(speed);
}

void go_back(int speed){
    set_reverse();
    set_throttle(speed);
}

void stop(){
    stop_throttle();
    set_neutral();
}

void set_forward(){
  digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
  digitalWrite(FORWARD_DIRECTION_PIN, LOW);
}

void set_reverse(){
  digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
  digitalWrite(REVERSE_DIRECTION_PIN, LOW);
}

void set_neutral(){
  digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
  digitalWrite(REVERSE_DIRECTION_PIN, HIGH);
}

void set_steering(long steering_raw) {
  dxl.setGoalPosition(DXL_ID, steering_raw+zero_offset);
}

channel_outputs pwm_to_outputs(int pwm){
  channel_outputs pwm_out;
  float out_voltage_a = 0.;
  float out_voltage_b = 0.;
  int out_pwm_a;
  int out_pwm_b;
  out_voltage_a = mapping(pwm, 0., 255., CHAN_A_MIN_VOLTAGE, CHAN_A_MAX_VOLTAGE);
  out_voltage_b = mapping(pwm, 0, 255, CHAN_B_MIN_VOLTAGE, CHAN_B_MAX_VOLTAGE);
  Serial.print(" PWM: ");
  Serial.print(pwm);
  Serial.print(" T_A: ");
  Serial.print(out_voltage_a);
  Serial.print(" T_B: ");
  Serial.print(out_voltage_b);
  pwm_out.chan_a = out_voltage_a;
  pwm_out.chan_b = out_voltage_b;
  return (pwm_out);
}

void throttle(int32_t throttle_pwm) {
  if (throttle_pwm < 1600) {
    stop();
  } else {
    throttle_in_mapped = map(throttle_pwm, LOWEST_PULSE_THROTTLE, HIGHEST_PULSE_THROTTLE, 0, 1000);
    if (throttle_in_mapped >= 400 && throttle_in_mapped <= 600) {stop();}
    else if (throttle_in_mapped > 600) {
      throttle_speed = map(throttle_in_mapped, 600, 1000, 0, 255);
      go_forward(throttle_speed);
    } else if (throttle_in_mapped < 400) {
      throttle_speed = map(throttle_in_mapped, 0, 400, 0, 255);
      throttle_speed = 255 - throttle_speed;
      go_back(throttle_speed);
    }
  }
}

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, -7796, 7796);
    Serial.print(" GT: ");
    Serial.print(steering_in_mapped);
    if (steering_in_mapped >= -200 && steering_in_mapped <= 200) {steering_zero();}
    else {
      set_steering(steering_in_mapped);
    }
  }
}

void check_trim_pins(){
  if (digitalRead(TRIM_RIGHT) == LOW) {increment_trim(TRIM_DELTA); delay(100);}
  if (digitalRead(TRIM_LEFT) == LOW) {decrement_trim(TRIM_DELTA); delay(100);}
}


void setup() {
  delay(1000);                                                            // Give time for Dynamixel to start on power-up
  pinMode(STEERING_INPUT, INPUT_PULLUP);
  pinMode(THROTTLE_INPUT, INPUT_PULLUP);
  pinMode(FORWARD_DIRECTION_PIN, OUTPUT);
  pinMode(REVERSE_DIRECTION_PIN, OUTPUT);
  pinMode(TRIM_RIGHT, INPUT_PULLUP);
  pinMode(TRIM_LEFT, INPUT_PULLUP);
  digitalWrite(FORWARD_DIRECTION_PIN, HIGH);
  digitalWrite(REVERSE_DIRECTION_PIN, HIGH);

  init_servo();

  DEBUG_SERIAL.begin(9600);
  delay(500);
  Serial.println("Starting...");

  long current_position = dxl.getPresentPosition(DXL_ID);
  zero_offset = current_position;
  Serial.print("Zero_offset: ");
  Serial.println(zero_offset);
  delay(100);

  Serial.print("Going to Zero (we should not move...): ");
  Serial.print(zero_offset);
  Serial.println(" Please wait for 2000ms...");
  dxl.setGoalPosition(DXL_ID, zero_offset); delay(2000);

  current_position = dxl.getPresentPosition(DXL_ID);
  Serial.print("now at: ");
  Serial.println(current_position);

  if (!mcp.begin()) {
    Serial.println("Failed to find MCP4728 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("Started.");
}

void loop() {
  throttle_in = pulseIn(THROTTLE_INPUT, LOW);
  Serial.print(" TPIN: ");
  Serial.print(throttle_in);
  throttle(throttle_in);
  Serial.print(" || ");
  steering_in = pulseIn(STEERING_INPUT, LOW);
  Serial.print(" SPIN: ");
  Serial.print(steering_in);
  steering(steering_in);
  Serial.println();
  check_trim_pins();
}