constexpr int motor_pin{12};
constexpr int dir_pin{16};
constexpr int limit_switch{22};
constexpr int us_trig = 17;
constexpr int us_echo = 27;


class EnergyController{
public:

    EnergyController(double k_energy, double upright_threshold=0.2, double length=0.2): k_energy(k_energy), upright_threshold(upright_threshold), length(length){
      bar_mass = calculate_bar_mass(0.005, length);
      inertia = calculate_polar_inertia(bar_mass, length);
      std::cout << "k_energy " << k_energy << std::endl;
      previous_output = k_energy*gravity;
    }
    double control_input = 0;
    double upright_threshold;
    double length;
    double bar_mass;
    double inertia;
    double gravity = 9.806;
    double k_energy;


    double previous_output;


    double calculate_bar_mass(double radius, double height){
      double density = 7850;
      double mass = density * 3.14159 * radius*radius * height;

      return mass;
    }


    double calculate_polar_inertia(double mass,double length){
      double inertia_pivot = (1 / 12) * mass * length*length;
      return inertia_pivot;
    }

    double total_energy(double angle, double angular_velocity){
      double potential_energy = -bar_mass * gravity * (length) * (std::cos(angle));
      double kinetic_energy = 0.5 * inertia * angular_velocity*angular_velocity;
      double total_energy = potential_energy;
      return total_energy;
    }

    double control(double angle, double angular_velocity){
      double reference_energy = total_energy(0, 0);

      double energy_error = total_energy(angle, angular_velocity) - reference_energy;
      double abs_ang_vel =std::abs(angular_velocity);
      if (abs_ang_vel == 0) {
        return previous_output;
      }
      if ((energy_error*angular_velocity*std::cos(angle))>0 ) {
        previous_output = -k_energy*gravity;
        return -k_energy*gravity;
      } else {
        previous_output = k_energy*gravity;
        return k_energy*gravity;
      }
    }
  };


void output_to_motor(double output) {
  gpioWrite(dir_pin, output > 0);
  output = abs(output)+ 25;
  if (output > 255)
    output = 255;
  gpioPWM(motor_pin, output);
}

bool loop_should_stop{false};

void limitSwitchISR(int gpio, int level, uint32_t tick) {
  std::cout << "ISR ENTERED" << std::endl;
  loop_should_stop = true;
  gpioPWM(motor_pin, 0);
}


// Convert AS5600 output to radians where 0 is vertically upright position. PI is the hang down position.
double uart_to_radians(int uart_value, int reference_angle) {
  const auto diff = 4096 - reference_angle;
  return static_cast<double>(uart_value + diff)*2.0*3.14159/4096.0;
}


int main(int argc, char** argv)
{
    AS_5600 rotary_encoder{}; // stuff we wrote to get AS5600 reading. Wrapper for reading from UART
    gpioInitialise();
    gpioSetMode(limit_switch, PI_INPUT);
    gpioSetPullUpDown(limit_switch, PI_PUD_DOWN);
    gpioSetISRFunc(limit_switch, RISING_EDGE, 0, limitSwitchISR);


    gpioInitialise();
    gpioSetMode(motor_pin, PI_OUTPUT);
    gpioSetMode(dir_pin, PI_OUTPUT);
    gpioSetPWMfrequency(motor_pin, 2000);

    auto t_old = std::chrono::steady_clock::now();


    // argv[1] is our k. We currently picked a k of 6
    EnergyController ec{std::stod(argv[1])};


    pin_me_to_core(1); // our function that pins program to core
    set_my_sched_fifo_priority(2); // our function that gives program a real time priority

    int reference_angle = std::stoi(argv[2]); // upright position that the AS5600 measures
    double prev_angle = uart_to_radians(rotary_encoder.getAngleUART(), reference_angle);
    while(!loop_should_stop)
    {
      auto start = std::chrono::steady_clock::now();
      double angle = uart_to_radians(rotary_encoder.getAngleUART(), reference_angle);


      auto d_angle = angle - prev_angle;



      const auto t_new = std::chrono::steady_clock::now();


      auto dt = std::chrono::duration<double>(t_new - t_old).count();
      auto angular_vel = d_angle / dt;


      auto output = ec.control(angle, angular_vel);


      output_to_motor(output);


      t_old = t_new;
      prev_angle = angle;


      auto end = std::chrono::steady_clock::now();
      using namespace std::chrono_literals;
      const auto min_dt{100ms}; // also implicitly the sampling delay
      const std::chrono::duration<double, std::nano> time_spent{end - start};
      // basically want to run every 100ms
      if (time_spent > min_dt) {
        std::cerr << "Skipping sleep\n";
      } else {
        std::this_thread::sleep_for(min_dt - time_spent);
      }


    }
    gpioPWM(motor_pin, 0);
    gpioTerminate();
}