/*
 * 	choketester.c 
 *      corresponds to PCB choketester.pro
 *
 * 	Copyright (C) 2022 Jiri Pittner <jiri@pittnerovi.com>
 *
 * 	This program is free software: you can redistribute it and/or modify
 * 	it under the terms of the GNU General Public License as published by
 * 	the Free Software Foundation, either version 3 of the License, or
 * 	(at your option) any later version.
 *
 * 	This program is distributed in the hope that it will be useful,
 * 	but WITHOUT ANY WARRANTY; without even the implied warranty of
 * 	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * 	GNU General Public License for more details.
 * 	You should have received a copy of the GNU General Public License
 * 	along with this program.  If not, see <http://www.gnu.org/licenses/>.
 * 	                                         
 * 	                                        
*/

#include "backward.h"

#include <avr/io.h>
#include <avr/pgmspace.h>
#include <avr/eeprom.h>
#include <avr/signal.h>
#include <avr/interrupt.h>
#include <setjmp.h>
#include <avr/sleep.h>
#include <stdlib.h>
#include <avr/wdt.h>
#include <string.h>
#include <math.h>



//hardware: atmega128, 14.7456MHz, kicad choketester.pro

#if defined(at90s2313) || defined(at90s8535)
#else
#define ATmega
#endif

#ifdef ATmega
#define USR UCSR1A
#endif


#define BAUD 115200

#if (XTAL == 14745600L) 
#define oversampling 4
#else
#define oversampling 2
#endif

#ifndef MCU
#define emulate
#else
#define assembler
#endif

#ifdef emulate
#define uint8_t unsigned char
#define uint16_t unsigned short
#define uint32_t unsigned int
#define uint64_t unsigned long long
#define int16_t  short
#define PSTR(X) (X)
#define fputs_P fputs
#define printP print
#define strcpy_P strcpy
#define strcat_P strcat
#define print(X) fputs(X,stdout)
#define scan(X,Y) fgets(X,(Y)-(X),stdin)
#define itoa10(N,S) sprintf(S,"%d",N)
#define itoa16(N,S) sprintf(S,"%x",N)
#else
#define itoa10(N,S) itoa(N,S,10)
#define itoa16(N,S) itoa(N,S,16)
#endif



#define itoa10(N,S) itoa(N,S,10)
#define itoa16(N,S) itoa(N,S,16)


void printP (PGM_P string){
        char c;
        c=pgm_read_byte(string);
                   while (c) {
                   loop_until_bit_is_set(UCSR1A, UDRE);
                   UDR1 = c;
                   c=pgm_read_byte(++string);
                   }
                   return;
                 }



void print (char *string){                                     
                   while (*string) {                                            
                   loop_until_bit_is_set(UCSR1A, UDRE);                            
                   UDR1 = *string++;
                   }                                                            
                   return;                                                      
                 }                                                              


//UART routines
volatile uint8_t inuartlen=0;
volatile uint8_t inuartline=0;
#define MAXUART 64
volatile char inuart[MAXUART];

INTERRUPT(SIG_UART1_RECV)
{
char c=UDR1;
if(inuartline) return; //ignore until line is processed
UDR1 = c; //echo
if(c=='\n' || c== '\r' || inuartlen == MAXUART-1) {inuartline=1; inuart[inuartlen]=0; inuartlen=0;} 
else inuart[inuartlen++]=c; 
}




#define UART_INIT(baud) { \
UBRR1H=0; \
UBRR1L= (XTAL/baud+15)/16-1; \
UCSR1B=(1<<TXEN)|(1<<RXEN); \
UCSR1C=(1<<UCSZ0)|(1<<UCSZ1)|(1<<USBS); }

//delay routines
void delay_xs(uint16_t xs) __attribute__((noinline));

void delay_xs(uint16_t xs) //xs takes 4 clocks time
{
        asm volatile (
        "\n"
        "L_dl1%=:" "\n\t"
        "sbiw %0, 1" "\n\t"
        "brne L_dl1%=" "\n\t"
        : "=&w" (xs)
        );
        return;
}

void delay_ms(uint16_t ms) __attribute__((noinline));

void delay_ms(uint16_t ms) //ms takes 1/1000 s
{
while(ms--) delay_xs(XTAL/4/1000);
}

void delay_us(uint16_t us)
{
delay_xs(XTAL*us/4/1000000);
}


;


//display routines

#define lcd_delay 50
volatile uint8_t light=0;

void lcd_w4bit(uint8_t rs, uint8_t x)
{
PORTC= 1<<PC4|light
	 | rs <<PC6;
delay_xs(lcd_delay);
PORTC |= x&0x0f;
delay_xs(lcd_delay);
cbi(PORTC,PC4);
delay_xs(lcd_delay);
sbi(PORTC,PC4);
delay_xs(lcd_delay);
}

uint8_t lcd_r4bit(uint8_t rs)
{
uint8_t r;
PORTC= 1<<PC4 | 1<<PC5|light
         | rs <<PC6;
delay_xs(lcd_delay);
cbi(PORTC,PC4);
delay_xs(lcd_delay);
r=PINC&0x0f;
cbi(PORTC,PC5);
delay_xs(lcd_delay);
return r;
}

uint8_t lcd_rbyte(uint8_t rs)
{
DDRC=0xf0;
delay_xs(lcd_delay);
return (lcd_r4bit(rs)<<4)|lcd_r4bit(rs);
DDRC=0xff;
delay_xs(lcd_delay);
}

void lcd_init4bit(void)
{
DDRC=0xff;
delay_xs(20000);
lcd_w4bit(0,3);
delay_xs(10000);
lcd_w4bit(0,3);
delay_xs(500);
lcd_w4bit(0,3);
lcd_w4bit(0,2);
}

void lcd_wbyte(uint8_t rs, uint8_t x)
{
lcd_w4bit(rs,x>>4);
lcd_w4bit(rs,x);
}

void lcd_clear(void)
{
lcd_wbyte(0,0x01);
delay_xs(30000);
}

void lcd_init(void)
{
lcd_init4bit();
lcd_wbyte(0,0x28);
lcd_wbyte(0,0x08);
lcd_clear();
lcd_wbyte(0,0x06);
lcd_wbyte(0,0x0c);
}

void lcd_print(char *t)
{
while(*t) if(*t=='\n') {++t; lcd_wbyte(0,0xc0);} else lcd_wbyte(1,*t++);
}


void lcd_print8(char *t)
{
uint8_t l=0;
while(*t) {lcd_wbyte(1,*t++); ++l;}
while(l<8) {lcd_wbyte(1,' '); ++l;}
}

void lcd_cursor(uint8_t r, uint8_t c)
{
lcd_wbyte(0,0x80+c+(r<<6));
}


#define Vcc 5000

uint16_t read_mv(uint8_t mux)
{
//Vcc=Vref
ADMUX=0x40|mux;
ADCSR=0x80|0x40|0x20|7;
uint32_t v;
v=ADCL;
v|= (ADCH<<8);
v*= Vcc;
v>>=10;
return v&0xffff;
}

float vbat()
{
float v=read_mv(3)*.001f;
v*= (150000.+33000.)/33000.; //voltage divisor
return v;
}



void pwm_set(uint16_t stride, uint16_t top)
{
sbi(DDRE,PE5);
OCR3C=stride;
ICR3=top;
TCNT3=0;
TCCR3B=1|(1<<WGM32)|(1<<WGM33);
TCCR3A=(1<<COM3C1)|(1<<WGM31);
}


static uint16_t old_v=0xffff;
void set_mv(uint16_t v, uint16_t top)
{
if(v==old_v) return;
old_v=v;
uint32_t stride = v;
stride *= top;
stride /= 5000;
pwm_set(stride&0xffff,top);
delay_ms(200); //wait until the voltage settles - might need to change it according to used resistors and capacitors in the RC filter
}


volatile uint16_t nrising;
volatile uint8_t virgin;
volatile uint16_t t_samples;
volatile uint16_t t_average;
volatile uint16_t max_average;
volatile uint32_t t_sum;

INTERRUPT(SIG_COMPARATOR)
{
//sbi(PORTB,PB6);
++nrising;
//cbi(PORTB,PB6);
}

INTERRUPT(SIG_INPUT_CAPTURE1)
{
//do not use ICR1, the interrupts are delayed between each other and there is a time shift
//measure interval between the same interrupt, i.e. zero and read TCNT1 at the same place
//sbi(PORTB,PB7);
uint16_t t=TCNT1;
TCNT1=0;
if(!virgin)
	{
	++t_samples;
	if(t_average<max_average)
		{
		++t_average;
		t_sum += t;
		}
	}
virgin=0;
//cbi(PORTB,PB7);
}

#define PWM_TOP 1000

//period is in nanosec
uint8_t measure1(uint16_t mv, uint16_t charge, uint16_t discharge, uint8_t prescale, uint16_t maxav,  uint16_t *nrise, uint16_t *nsamples, uint32_t *period)
{
set_mv(mv,PWM_TOP);
max_average=maxav;
cbi(PORTB,PB4);
delay_ms(charge);
//setup timer
t_samples=0;
t_average=0;
t_sum=0;
virgin=1;
TCCR1A=0;
TCCR1C=0;
uint8_t prescale_code=1;
if(prescale==8) prescale_code=2;
else if(prescale==64) prescale_code=3;
else {prescale=1;prescale_code=1;}
TCCR1B=prescale_code|(1<<ICES1)|(1<<ICNC1);
sbi(TIMSK,TICIE1);

nrising=0;
TCNT1=0;
//setup comparator
ACSR = (1<<ACIE)|(1<<ACIC)|(1<<ACIS1)|(1<<ACIS0);
sbi(PORTB,PB4);
delay_ms(discharge);

//stop interrupts
cbi(TIMSK,TICIE1);
cbi(ACSR,ACIC);
cbi(PORTB,PB7);
if(t_samples==nrising && t_samples>1) //this was a glitch
	{
	--t_samples;
	if(t_samples<t_average) t_average=t_samples;
	}
if(t_average) *period = ((t_sum*1000000000LL)/t_average)*prescale/XTAL;
else {*nsamples=0; return 2;}
*nsamples= t_samples;
*nrise=nrising;
//if(nrising<=1 || t_samples<nrising-2 || t_samples>nrising) return 1; //too many glitches - unreliable flag
return 0;
}


//estimate the Q-factor (cf. https://en.wikipedia.org/wiki/Q_factor)
//as Q = pi *n_periods / ln(V0/V) where the voltage oscillation amplitude
//drops from V0 to V within n oscillation periods
//perform two measurements at different voltage thresholds and compute Q

uint8_t measureLQ(uint16_t nperiods, float *L, float *Q)
{
const float capacitance=99.6e-9f;
//const float Q_C = 800;
char c[64];
//make first measurement for orientation
uint32_t period=0;
uint16_t nsamples;
uint16_t nrise;
uint8_t prescale=8;
uint16_t mv=300;
uint8_t r=measure1(mv,50,200,prescale,10,&nrise,&nsamples,&period);
if(r>=2) return r;
float freq=1e9/period;
float inductance = 1.f/(4*M_PI*M_PI*freq*freq*capacitance);
if(inductance<0.01f) prescale=1;
//sprintf(c,"prescale %d\n",prescale); print(c);

//preliminary value
*L = inductance;

//find voltage threshold yielding just a single period
mv=2500;
r=measure1(mv,50,200,prescale,10,&nrise,&nsamples,&period);
while(nsamples!=1)
	{
	if(nsamples>10) mv+=100;
	else if(nsamples>4) mv+=50;
	else {if(nsamples>1) mv += 20;}
	if(mv>=4900) return 1;
	r=measure1(mv,50,200,prescale,10,&nrise,&nsamples,&period);
	if(nsamples<1) mv/=2;
	}
mv += 20; //this was the last voltage where we had 2 periods
float v_1=.001f*mv;
r=measure1(mv,50,200,prescale,10,&nrise,&nsamples,&period);
if(r>=2) return r;
sprintf(c,"%d periods @ %.3fV\n",nsamples+1,v_1); print(c);

mv=300;
do
	{
	r=measure1(mv,50,200,prescale,nperiods,&nrise,&nsamples,&period);
	if(r>=2||nsamples==0) return 2;
	if(nsamples==1+nperiods) break;
	if(nsamples>10+nperiods) mv+=100;
        else if(nsamples>4+nperiods) mv+=50;
        else mv += 20;
	if(mv>=4900) return 1;
	}
while(nsamples>1+nperiods);

mv += 20; //this was the last voltage where we had 2+nperiods
r=measure1(mv,50,200,prescale,nperiods,&nrise,&nsamples,&period);
if(r) return r;
float v_nperiods=.001f*mv;
sprintf(c,"%d periods @ %.3fV\n",nsamples+1,v_nperiods); print(c);

freq=1e9/period;
inductance = 1.f/(4*M_PI*M_PI*freq*freq*capacitance);
*L = inductance;
if(nsamples==1)
	{
	*Q = 0;
	return 1;
	}
//we can compute Q
float q = M_PI * (nsamples+1 - 2) / logf(v_1/v_nperiods);

//TODO: how to adjust Q to compensate for capacitor and FET losses?
*Q = q;

return 0;
}


void init_led()
{
cbi(PORTB,PB5);
cbi(PORTB,PB6);
cbi(PORTB,PB7);
sbi(DDRB,PB5);
sbi(DDRB,PB6);
sbi(DDRB,PB7);
}


int main()
{
sbi(SREG,7);
UART_INIT(BAUD);
//allow uart interrupts
{char dummy=UDR1;}
sbi(UCSR1B,RXCIE1);


printP(PSTR("Reset!\n"));
lcd_init();
lcd_print("Battery ");

(void)read_mv(3); //init adc
delay_ms(1);
float vb=vbat();
{
char c[16];
sprintf(c,"%5.2fV    \n",vb);
print(c);
lcd_cursor(1,0);
lcd_print8(c);
}
delay_ms(500);

cbi(PORTB,PB4);
sbi(DDRB,PB4);


uint16_t nperiods=10;



while(1) //main loop
	{
	float L,Q;
	uint8_t r=measureLQ(nperiods,&L,&Q);
	char c1[20],c2[20];
	if(r>=2) 
		{
		sprintf(c1,"N/C or   ");
		sprintf(c2,"FAIL    \n");
		nperiods=10;
		}
	else 
		{
		if(L<0.01f) sprintf(c1,"%4.0fuH",L*1e6f);
		else if(L<1.f) sprintf(c1,"%6.2fmH ",L*1e3f);
		else sprintf(c1,"%4.2fH    ",L);
		}
	if(r==1) sprintf(c2,"Q=low   \n");
	else  if(r==0)   sprintf(c2,"Q=%3.1f     \n",Q);
	print(c1); printP(PSTR("  ")); print(c2);
	lcd_cursor(0,0); lcd_print8(c1);
	lcd_cursor(1,0); lcd_print8(c2);
	if(r==0)
		{
		if(Q<10.f) nperiods=5;
		else  if(Q<20.f)  nperiods=10;
		else  nperiods=15;
		if(Q>40.f)  nperiods=20;
		else {if(Q>50.f) nperiods=30;}
		}


	if(inuartline)
		{
		switch(inuart[0]) 
			{
			case 'v':
				{
				uint16_t v=read_mv(1);
				char c[16];
				sprintf(c,"%d",v);
				printP(PSTR("V = "));
				print(c);
				printP(PSTR("\n"));
				}
				break;
                                break;
/*
			case 'V':
				{
				mvolts=atoi(inuart+1);
				set_mv(mvolts,pwmtop);
				}
				break;
                        case 'W':
                                {
                                pwmtop=atoi(inuart+1);
				set_mv(mvolts,pwmtop);
                                }
                                break;
*/
			default:
				printP(PSTR("unknown command"));
			};
		printP(PSTR("\n"));
		inuartline=inuartlen=0;
		}
	}
}