// Basic demo for configuring the MCP4728 4-Channel 12-bit I2C DAC
#include <Adafruit_MCP4728.h>
#include <Wire.h>

#define FORK_VERT_INPUT 15
#define LOWEST_PULSE 17770
#define HIGHEST_PULSE 18790

Adafruit_MCP4728 mcp;

static float CHAN_A_V_MIN_VOLTAGE = 1.04 + 0.0;
static float CHAN_A_V_MAX_VOLTAGE = 3.84 + 0.0;
static float CHAN_B_V_MIN_VOLTAGE = 0.88 - 0.0;
static float CHAN_B_V_MAX_VOLTAGE = 3.52 - 0.0;
static float STOP_VOLTAGE_V_A = 2.40 + 0.4;
static float STOP_VOLTAGE_V_B = 2.16 + 0.0;

int32_t fork_vert_in;
int32_t fork_vert_in_mapped;
int fork_vert_speed;
int mapped_fork_vert_chan_a;
int mapped_fork_vert_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;
}

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){
  int v_int;
  v_int = convert_to_digital(v);
  return v_int;
}
//Helper functions-----

void set_voltage_a(float v){
  int v_int = process_voltage(v);
  Serial.print(" || ");
  Serial.print(v);
  Serial.print(" | ");
  Serial.print(v_int);
  Serial.print(" || ");
  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(STOP_VOLTAGE_V_A);
  set_voltage_b(STOP_VOLTAGE_V_B);
  Serial.print(" VERT 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);
}

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

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 setup(void) {
  pinMode(FORK_VERT_INPUT, INPUT_PULLUP);
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  if (!mcp.begin()) {
    Serial.println("Failed to find MCP4728 chip");
    while (1) {
      delay(10);
    }
  }
}
//
//void loop() {
//  fork_vert_in = pulseIn(FORK_VERT_INPUT, LOW);
//  Serial.print(" VPIN: ");
//  Serial.print(fork_vert_in);
//  fork_vert(fork_vert_in);
//  Serial.println();
//  delay(1);
//  }


void loop() {
  set_voltage_a(0.0);
  set_voltage_b(0.0);
  delay(500);
  set_voltage_a(2.5);
  set_voltage_b(2.5);
  delay(500);
  set_voltage_a(5);
  set_voltage_b(5);
  delay(500);
  }