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

Adafruit_MCP4728 mcp;

#define MAX_V 5.2

#define FORK_VERT_INPUT 14
#define FORK_HORIZ_INPUT 15

#define LOWEST_PULSE 17770
#define HIGHEST_PULSE 18790
// Recorded: L: 17791 H: 18786
// #define PULSE_DELTA = 1100; // this doesn't change
// Measured voltages: V: LA:1.04 MA:2.40 HA:3.76 LB:0.80 MB:2.16 HB:3.52
// Measured voltages: H: LA:1.04 MA:2.40 HA:3.76 LB:0.96 MB:2.32 HB:3.60
/*
    CHAN_A IS VERT A
    CHAN_B IS VERT B
    CHAN_C IS HORIZ A
    CHAN_D IS HORIZ B
*/

static float CHAN_A_V_MIN_VOLTAGE = 1.04 + 0.0;
static float CHAN_A_V_MAX_VOLTAGE = 3.76 + 0.0;
static float CHAN_B_V_MIN_VOLTAGE = 0.80 - 0.0;
static float CHAN_B_V_MAX_VOLTAGE = 3.52 - 0.0;

static float CHAN_A_H_MIN_VOLTAGE = 1.04 + 0.0;
static float CHAN_A_H_MAX_VOLTAGE = 3.76 + 0.0;
static float CHAN_B_H_MIN_VOLTAGE = 0.96 + 0.0;
static float CHAN_B_H_MAX_VOLTAGE = 3.60 + 0.0;

static float STOP_VOLTAGE_V_A = 2.40 + 0.08;
static float STOP_VOLTAGE_V_B = 2.16 + 0.00;

static float STOP_VOLTAGE_H_A = 2.40 + 0.0;
static float STOP_VOLTAGE_H_B = 2.32 + 0.0;

int32_t fork_vert_in;
int32_t fork_horiz_in;
int32_t fork_vert_in_mapped;
int32_t fork_horiz_in_mapped;

int fork_vert_speed;
int fork_horiz_speed;

int mapped_fork_vert_chan_a;
int mapped_fork_vert_chan_b;
int mapped_fork_horiz_chan_a;
int mapped_fork_horiz_chan_b;

struct channel_outputs {
    double chan_a;
    double chan_b;
};

//helper functions 
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;
}

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 adjusted_v;
}

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;
}

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;
}
//helper functions 

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 set_voltage_c(float v){
  int v_int = process_voltage(v);
  mcp.setChannelValue(MCP4728_CHANNEL_C, v_int);
}
void set_voltage_d(float v){
  int v_int = process_voltage(v);
  mcp.setChannelValue(MCP4728_CHANNEL_D, v_int);
}

void stop_fork_vert() {
  set_voltage_a(low_voltage_compensator(STOP_VOLTAGE_V_A));
  set_voltage_b(low_voltage_compensator(STOP_VOLTAGE_V_B));
  Serial.print(" VERT STOPPED ");
}

void stop_fork_horiz() {
  set_voltage_c(low_voltage_compensator(STOP_VOLTAGE_H_A));
  set_voltage_d(low_voltage_compensator(STOP_VOLTAGE_H_B));
  Serial.print(" HORIZ STOPPED ");
}

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

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

void fork_left(int speed){
  channel_outputs pwm_out = pwm_to_outputs_h_high(speed);
  set_voltage_c(pwm_out.chan_a);
  set_voltage_d(pwm_out.chan_b);
}

void fork_right(int speed){
channel_outputs pwm_out = pwm_to_outputs_h_low(speed);
  set_voltage_c(pwm_out.chan_a);
  set_voltage_d(pwm_out.chan_b);
}

channel_outputs pwm_to_outputs_v_high(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., STOP_VOLTAGE_V_A, CHAN_A_V_MAX_VOLTAGE);
  out_voltage_b = mapping(255 - pwm, 0, 255, CHAN_B_V_MIN_VOLTAGE, STOP_VOLTAGE_V_B);
  Serial.print(" PWM: ");
  Serial.print(pwm);
  Serial.print(" HV_A: ");
  Serial.print(out_voltage_a);
  Serial.print(" HV_B: ");
  Serial.print(out_voltage_b);
  pwm_out.chan_a = out_voltage_a;
  pwm_out.chan_b = out_voltage_b;
  return (pwm_out);
}

channel_outputs pwm_to_outputs_v_low(int pwm){
  channel_outputs pwm_out;
  float out_voltage_a;
  float out_voltage_b;
  int out_pwm_a;
  int out_pwm_b;
  out_voltage_a = mapping(255 - pwm, 0., 255., CHAN_A_V_MIN_VOLTAGE, STOP_VOLTAGE_V_A);
  out_voltage_b = mapping(pwm, 0., 255., STOP_VOLTAGE_V_B, CHAN_B_V_MAX_VOLTAGE);
  Serial.print(" PWM: ");
  Serial.print(pwm);
  Serial.print(" LV_A: ");
  Serial.print(out_voltage_a);
  Serial.print(" LV_B: ");
  Serial.print(out_voltage_b);
  pwm_out.chan_a = out_voltage_a;
  pwm_out.chan_b = out_voltage_b;
  return (pwm_out);
}

channel_outputs pwm_to_outputs_h_low(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., STOP_VOLTAGE_H_A, CHAN_A_H_MAX_VOLTAGE);
  out_voltage_b = mapping(255 - pwm, 0, 255, CHAN_B_H_MIN_VOLTAGE, STOP_VOLTAGE_H_B);
  Serial.print(" PWM: ");
  Serial.print(pwm);
  Serial.print(" HV_A: ");
  Serial.print(out_voltage_a);
  Serial.print(" HV_B: ");
  Serial.print(out_voltage_b);
  pwm_out.chan_a = out_voltage_a;
  pwm_out.chan_b = out_voltage_b;
  return (pwm_out);
}

channel_outputs pwm_to_outputs_h_high(int pwm){
  channel_outputs pwm_out;
  float out_voltage_a;
  float out_voltage_b;
  int out_pwm_a;
  int out_pwm_b;
  out_voltage_a = mapping(255 - pwm, 0., 255., CHAN_A_H_MIN_VOLTAGE, STOP_VOLTAGE_H_A);
  out_voltage_b = mapping(pwm, 0., 255., STOP_VOLTAGE_H_B, CHAN_B_H_MAX_VOLTAGE);
  Serial.print(" PWM: ");
  Serial.print(pwm);
  Serial.print(" LV_A: ");
  Serial.print(out_voltage_a);
  Serial.print(" LV_B: ");
  Serial.print(out_voltage_b);
  pwm_out.chan_a = out_voltage_a;
  pwm_out.chan_b = out_voltage_b;
  return (pwm_out);
}

void fork_vert(int32_t vert_pwm) {
  if (vert_pwm < 17000) {
    stop_fork_vert();
  } else {
    fork_vert_in_mapped = map(vert_pwm, LOWEST_PULSE, HIGHEST_PULSE, 0, 1000);
    if (fork_vert_in_mapped >= 400 && fork_vert_in_mapped <= 600) {stop_fork_vert();}
    else if (fork_vert_in_mapped > 600) {
      fork_vert_speed = map(fork_vert_in_mapped, 600, 1000, 0, 255);
      fork_up(fork_vert_speed);
    } else if (fork_vert_in_mapped < 400) {
      fork_vert_speed = map(fork_vert_in_mapped, 0, 400, 0, 255);
      fork_vert_speed = 255 - fork_vert_speed;
      fork_down(fork_vert_speed);
    }
  }
}

void fork_horiz(int32_t horiz_pwm) {
  if (horiz_pwm < 17000) {
    stop_fork_horiz();
  } else {
    fork_horiz_in_mapped = map(horiz_pwm, LOWEST_PULSE, HIGHEST_PULSE, 0, 1000);
    if (fork_horiz_in_mapped >= 400 && fork_horiz_in_mapped <= 600) {stop_fork_horiz();}
    else if (fork_horiz_in_mapped > 600) {
      fork_horiz_speed = map(fork_horiz_in_mapped, 600, 1000, 0, 255);
      fork_right(fork_horiz_speed);
    } else if (fork_horiz_in_mapped < 400) {
      fork_horiz_speed = map(fork_horiz_in_mapped, 0, 400, 0, 255);
      fork_horiz_speed = 255 - fork_horiz_speed;
      fork_left(fork_horiz_speed);
    }
  }
}


void setup() {
  pinMode(FORK_VERT_INPUT, INPUT_PULLUP);
  pinMode(FORK_HORIZ_INPUT, INPUT_PULLUP);
  Serial.begin(9600);
  delay(2000);
  Serial.println("Starting...");
  if (!mcp.begin()) {
    Serial.println("Failed to find MCP4728 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("Started.");
}

void loop() {
  fork_vert_in = pulseIn(FORK_VERT_INPUT, LOW);
  Serial.print(" VPIN: ");
  Serial.print(fork_vert_in);
  fork_vert(fork_vert_in);
  Serial.print(" || ");
  fork_horiz_in = pulseIn(FORK_HORIZ_INPUT, LOW);
  Serial.print(" HPIN: ");
  Serial.print(fork_horiz_in);
  fork_horiz(fork_horiz_in);
  Serial.println();
  delay(1);
}