#include "WS2812.h"

#define numleds 2
uint8_t pixel[numleds*3];

#define PWM_CT1_B  10 //OC1B
#define PWM_CT2_B   3 //OC2B

#define Vout  A1 
#define Vin   A0 

#define maxpwm 4*248
#define minpwm  0

float V_set_in  = 17.0;
float V_set_out = 14.4;
int measct;
int calibct;

void ct1_init()
{
  pinMode(PWM_CT1_B,OUTPUT);
  // 64 khz
  TCCR1A = (1 << COM1B0)|(1 << COM1B1)|(1 << WGM11)|(1 << WGM10);  //fast PWM INV 10bit
  TCCR1B = (1 << CS10)|(1 << WGM12);       // prescaler 0
  OCR1B = 1;                          // pwm value  
}

void setup() 
{
  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(Vout, INPUT);
  pinMode(Vin, INPUT);
  Serial.begin(115200);
  ws2812_init();
  ct1_init();
  measct = 50;
}

int V_out, V_in;
uint16_t pwm;
float fpwm;
char in_ch;
uint8_t ledct;
  
void loop() 
{ 
  float V_out_f, V_in_f;
  float in_diff, out_diff;
    
  V_in  = analogRead(Vin); // 18.02 610
  V_out = analogRead(Vout);// 14.20 476

  V_in_f  = V_in  * 0.0295;
  V_out_f = V_out * 0.0298;

  in_diff  = V_set_in  - V_in_f;
  out_diff = V_set_out - V_out_f;

  if (out_diff <= -0.1) // batt volt high
  { 
    fpwm += 3.5 * out_diff; 
    digitalWrite(LED_BUILTIN,HIGH);
  }
  else                  // follow Vin
  { 
    fpwm -= 2.0 * in_diff;  
    digitalWrite(LED_BUILTIN,LOW); 
  }
    
  fpwm = constrain(fpwm,minpwm,maxpwm);
  if (measct > 0) 
  {
    pwm = minpwm;
    measct--;
    if (measct == 0) 
    {
      V_set_in = 0.8 * V_in_f;
      fpwm = 0.0;
    }
  }
  else
    pwm = (uint16_t)fpwm;
 
  OCR1B = pwm;
         
  if (Serial.available())
  {
    in_ch = Serial.read();
    if (in_ch == '1') V_set_in -= 0.5;
    if (in_ch == '2') V_set_in += 0.5;
    if (in_ch == '3') V_set_out -= 0.1;
    if (in_ch == '4') V_set_out += 0.1;
    if (in_ch == '5') measct = 50;
  }
  
  //Serial.print(V_in); Serial.print(", ");
  //Serial.print(V_out); Serial.print(", ");
  Serial.print((float)pwm/32.0); Serial.print(", ");
  Serial.print(V_set_in); Serial.print(", "); 
  Serial.print(V_in_f); Serial.print(", "); 
  //Serial.print(in_diff); Serial.print(", "); 
  //Serial.print(V_in); Serial.print(", ");
  Serial.print(V_set_out); Serial.print(", "); 
  Serial.print(V_out_f); Serial.print(", "); 
  //Serial.print(out_diff); Serial.print(", "); 
  //Serial.print(V_out); Serial.print(", "); 

  Serial.println();

  ledct++;
  if (ledct >= 15)  // 300ms
  {
    ledct = 0;
    for (uint8_t i=0;i<3*numleds;i++) pixel[i] = 0;  
    if (out_diff > 0.2) pixel[4]=0x5a; // bat volt   
    else                pixel[3]=0x5a;    
    if (in_diff  > 0.1) pixel[1]=0x5a; // in volt 
    else                pixel[0]=0x5a;
    if (measct > 0)     pixel[2]=0x5a; 
  
    ws2812_sendarray(pixel,numleds*3);
    
    calibct++;
    if (calibct > 2000) // 10 min 
    {
      measct=50;
      calibct = 0;
    }
  }

  delay(20);
}