#include <Arduino.h>
#include <stdint.h>

#include "control.h"
#include "ctrlParams.h"
#include "pwm.h"
#include "hardware.h"






volatile Control ctrl((float)PWM_MIN, (float)PWM_MAX, Ctrl_P, Ctrl_I, Ctrl_D);

volatile float u_des = 0;
volatile float u_curr = 0;
volatile uint16_t newPwm = 0;

float getP() {
	return ctrl.getP();
}

void setP(float p) {
	ctrl.setP(p);
}

float getI() {
	return ctrl.getI();
}

void setI(float i) {
	ctrl.setI(i);
}

float getD() {
	return ctrl.getD();
}

void setD(float d) {
	ctrl.setD(d);
}

float getDutyCycle() {
  noInterrupts();
  uint16_t my_newPwm = newPwm;
  interrupts();
  float dutyCycle = ((float)my_newPwm) / ((float)PWM_MAX) * 100.0;
  return dutyCycle;
}

float getUCur() {
  noInterrupts();
  float my_u_curr = u_curr;
  interrupts(); 
  return my_u_curr;
}

float getUDes() {
  noInterrupts();
  float my_u_des = u_des;
  interrupts(); 
  return my_u_des;
}

void setUDes(float uDes) {
  if (uDes < 0) {
    uDes= 0;
  }
  noInterrupts();
  u_des = uDes;
  interrupts();
}

void cycle() {
	static uint32_t cycles = 0;
	if (cycles >= CYCLE_DIV) {
		cycles = 0;

		uint16_t adcIn = analogRead(ADC_IN);
		float u_adc = ((float)adcIn) * U_ref / ((float)ADC_MAX);
		u_curr = u_adc * (R_top + R_bottom) / R_bottom;

		float newPwm_f = ctrl.cycle(u_des, u_curr);
		newPwm = (uint16_t) newPwm_f;

		analogWrite(PWM_PIN, newPwm);
	}
	cycles++;
}


void pwmInit() {
  pinMode(PWM_PIN, OUTPUT);
	analogWrite(PWM_PIN, PWM_MIN);
	analogWriteFrequency(PWM_PIN, PWM_FREQ);
	analogWriteResolution(PWM_RES);
	pinMode(PWM_LOOPBACK, INPUT);
	attachInterrupt(PWM_LOOPBACK, cycle, RISING);

	analogReadResolution(ADC_RES);
	analogReference(DEFAULT);
	if (ADC_AVG != 0) {
		analogReadAveraging(ADC_AVG);
	}
	pinMode(ADC_IN, INPUT);
}