#include "stm32g4xx.h"
#include "drill_dual_adc.h"
#include "drill_adc_capture.h"
#include "drill_pwm_stage.h"
#include "drill_spi_transport.h"
#include "drill_config_binding.h"
#include "drill_timebase.h"
#include <stddef.h>
#define DU_COMMON (ADC_CCR_CKMODE|ADC_CCR_DUAL_2|ADC_CCR_DUAL_1)
#define DU_TRIGGER (ADC_CFGR_EXTSEL_3|ADC_CFGR_EXTSEL_1|ADC_CFGR_EXTEN_0)
#define DU_FLAGS (ADC_ISR_OVR|ADC_ISR_JQOVF|ADC_ISR_AWD1|ADC_ISR_AWD2|ADC_ISR_AWD3|ADC_ISR_JEOC|ADC_ISR_JEOS)
volatile du_state drill_dual_state;
volatile uint32_t drill_dual_error;
static uint32_t du_last,du_phase,du_origin,du_sequence;
static du_sample du_pending;
static bool du_regs(ADC_TypeDef *p,unsigned channel,bool master){
 if(p->CFGR!=(master?DU_TRIGGER:0u)||p->CFGR2||p->IER||p->DIFSEL||p->JSQR||p->SMPR1!=(2u<<(3u*channel))||p->SMPR2)return false;
 if(p->SQR1!=(channel<<6)||p->SQR2||p->SQR3||p->SQR4||p->OFR1||p->OFR2||p->OFR3||p->OFR4||p->GCOMP)return false;
 uint32_t expect=ADC_CR_ADVREGEN,allowed=0;
 if(drill_dual_state==DU_CAL)allowed=ADC_CR_ADCAL;
 if(drill_dual_state>=DU_ENABLE&&drill_dual_state<=DU_DONE)expect|=ADC_CR_ADEN;
 if(drill_dual_state==DU_WAIT)allowed=ADC_CR_ADSTART;
 if(drill_dual_state==DU_STOP)allowed=ADC_CR_ADSTART|ADC_CR_ADSTP;
 return (p->CR&~allowed)==expect;
}
static bool du_ok(void){
 return ((TIM1->CR1&TIM_CR1_CEN)!=0)==(drill_dual_state==DU_WAIT)&&drill_adc_state==DA_IDLE&&drill_config.state==DC_READY&&drill_spi_available()&&drill_pwm_sampling_available()&&
 (GPIOF->ODR&2u)&&(GPIOF->IDR&2u)&&((GPIOA->MODER>>12)&3u)==0u&&(GPIOA->IDR&(1u<<6))&&
 ((GPIOA->MODER>>24)&3u)==1u&&!(GPIOA->OTYPER&(1u<<12))&&(GPIOA->ODR&(1u<<12))&&(GPIOA->IDR&(1u<<12))&&
 (GPIOA->MODER&15u)==15u&&!(GPIOA->PUPDR&15u)&&(RCC->AHB2ENR&RCC_AHB2ENR_ADC12EN)&&!(RCC->AHB2RSTR&RCC_AHB2RSTR_ADC12RST)&&
 ADC12_COMMON->CCR==DU_COMMON&&du_regs(ADC1,1,true)&&du_regs(ADC2,2,false);
}
static void du_fail(unsigned reason){
 drill_spi_abort();drill_pwm_sampling_abort();drill_config.state=DC_FAILED;drill_config.error=DC_INTERLOCK;
 if(RCC->AHB2ENR&RCC_AHB2ENR_ADC12EN){RCC->AHB2RSTR|=RCC_AHB2RSTR_ADC12RST;__DSB();RCC->AHB2RSTR&=~RCC_AHB2RSTR_ADC12RST;__DSB();RCC->AHB2ENR&=~RCC_AHB2ENR_ADC12EN;}
 du_pending=(du_sample){0};drill_dual_error=reason;drill_dual_state=DU_FAILED;
}
static void du_prepare(ADC_TypeDef *p,unsigned channel,bool master){
 p->CFGR=master?DU_TRIGGER:0u;p->CFGR2=0;p->IER=0;p->DIFSEL=0;p->JSQR=0;p->SMPR1=2u<<(3u*channel);p->SMPR2=0;
 p->SQR1=channel<<6;p->SQR2=p->SQR3=p->SQR4=0;p->OFR1=p->OFR2=p->OFR3=p->OFR4=p->GCOMP=0;
 p->CR&=~ADC_CR_DEEPPWD;p->CR|=ADC_CR_ADVREGEN;
}
bool drill_dual_bench_begin(uint32_t now,bool qualified){
 if(!qualified||drill_dual_state!=DU_IDLE||drill_adc_state!=DA_IDLE||drill_config.state!=DC_READY||!drill_spi_available()||drill_spi_active())return false;
 if((RCC->AHB2ENR&RCC_AHB2ENR_ADC12EN)||(RCC->AHB2RSTR&RCC_AHB2RSTR_ADC12RST))return false;
 if(!drill_pwm_sampling_arm())return false;
 GPIOA->MODER|=15u;GPIOA->PUPDR&=~15u;__DSB();RCC->AHB2ENR|=RCC_AHB2ENR_ADC12EN;(void)RCC->AHB2ENR;__DSB();
 RCC->AHB2RSTR|=RCC_AHB2RSTR_ADC12RST;__DSB();RCC->AHB2RSTR&=~RCC_AHB2RSTR_ADC12RST;__DSB();
 ADC12_COMMON->CCR=DU_COMMON;du_prepare(ADC1,1,true);du_prepare(ADC2,2,false);__DSB();
 du_last=du_phase=du_origin=now;du_sequence=0;du_pending=(du_sample){0};drill_dual_error=0;drill_dual_state=DU_REG;
 if(!du_ok()){du_fail(1);return false;}return true;
}
void drill_dual_poll(uint32_t now){
 if(drill_dual_state==DU_IDLE||drill_dual_state==DU_FAILED)return;
 if(now-du_last>0x7fffffffu){du_fail(2);return;}du_last=now;
 if(!du_ok()){du_fail(1);return;}
 uint32_t a=ADC1->ISR,b=ADC2->ISR;if((a|b)&DU_FLAGS){du_fail(3);return;}
 if(drill_dual_state<DU_READY&&now-du_origin>5000u){du_fail(2);return;}
 if(drill_dual_state==DU_REG){if(now-du_phase>=40u){ADC1->CR|=ADC_CR_ADCAL;ADC2->CR|=ADC_CR_ADCAL;drill_dual_state=DU_CAL;}return;}
 if(drill_dual_state==DU_CAL){if(!((ADC1->CR|ADC2->CR)&ADC_CR_ADCAL)){drill_dual_state=DU_GAP;du_phase=now;}return;}
 if(drill_dual_state==DU_GAP){if(now-du_phase>=2u){ADC1->ISR=ADC_ISR_ADRDY;ADC2->ISR=ADC_ISR_ADRDY;__DSB();ADC2->CR|=ADC_CR_ADEN;ADC1->CR|=ADC_CR_ADEN;drill_dual_state=DU_ENABLE;}return;}
 if(drill_dual_state==DU_ENABLE){if((a&b)&ADC_ISR_ADRDY)drill_dual_state=DU_READY;return;}
 if(drill_dual_state==DU_WAIT){
  if(now-du_phase>40u){du_fail(2);return;}
  if(!((a&b)&ADC_ISR_EOC))return;
  if(!((a&b)&ADC_ISR_EOS)){du_fail(3);return;}
  /* Pause the internal timer before another period can replace this pair. */
  drill_pwm_sampling_pause();
  uint32_t paused;
  if(!drill_timebase_get(&paused)||paused-du_phase>40u){du_fail(2);return;}
  a=ADC1->ISR;b=ADC2->ISR;
  if(((a|b)&DU_FLAGS)||!((a&b)&ADC_ISR_EOC)||!((a&b)&ADC_ISR_EOS)){du_fail(3);return;}
  uint32_t raw=ADC12_COMMON->CDR;
  if(raw&0xf000f000u){du_fail(4);return;}
  du_pending.ia_raw=(uint16_t)raw;du_pending.ib_raw=(uint16_t)(raw>>16);du_pending.observed_us=paused;du_pending.timer_count=(uint16_t)TIM1->CNT;
  ADC1->ISR=ADC_ISR_EOC|ADC_ISR_EOS|ADC_ISR_EOSMP;ADC2->ISR=ADC_ISR_EOC|ADC_ISR_EOS|ADC_ISR_EOSMP;__DSB();
  if(ADC1->CR&ADC_CR_ADSTART)ADC1->CR|=ADC_CR_ADSTP;
  drill_dual_state=DU_STOP;du_phase=now;return;
 }
 if(drill_dual_state==DU_STOP){
  if(now-du_phase>100u){du_fail(2);return;}
  if((ADC1->CR|ADC2->CR)&(ADC_CR_ADSTART|ADC_CR_ADSTP))return;
  du_pending.sequence=++du_sequence;drill_dual_state=DU_DONE;
 }
}
bool drill_dual_capture_start(uint32_t now){
 if(drill_dual_state!=DU_READY)return false;
 drill_dual_poll(now);if(drill_dual_state!=DU_READY||drill_spi_active())return false;
 du_pending=(du_sample){.armed_us=now};ADC1->ISR=ADC_ISR_EOC|ADC_ISR_EOS|ADC_ISR_EOSMP;ADC2->ISR=ADC_ISR_EOC|ADC_ISR_EOS|ADC_ISR_EOSMP;__DSB();
 ADC1->CR|=ADC_CR_ADSTART;drill_dual_state=DU_WAIT;du_phase=now;
 if(!drill_pwm_sampling_start()){du_fail(1);return false;}return true;
}
bool drill_dual_take(du_sample *out,uint32_t now){
 if(!out||drill_dual_state!=DU_DONE)return false;
 drill_dual_poll(now);if(drill_dual_state!=DU_DONE)return false;
 if(now-du_pending.observed_us>100u){du_fail(2);return false;}*out=du_pending;du_pending=(du_sample){0};drill_dual_state=DU_READY;return true;
}
bool drill_dual_busy(void){return drill_dual_state!=DU_IDLE&&drill_dual_state!=DU_READY&&drill_dual_state!=DU_DONE&&drill_dual_state!=DU_FAILED;}
