#include "stm32g4xx.h"
#include "drill_current.h"
#include "drill_config_binding.h"
#include "drill_spi_transport.h"
#include "drill_timebase.h"
#include "drill_adc_capture.h"
#include <stddef.h>
_Static_assert(sizeof(cq_record)==80u,"current diagnostic ABI");
volatile cq_record drill_debug_current;
static uint32_t cq_origin,cq_last_poll,cq_last_sample,cq_sequence;
static uint32_t cq_sum_a,cq_sum_b,cq_lo,cq_hi,cq_zero_lo,cq_zero_hi,cq_scale;
static uint16_t cq_min_a,cq_max_a,cq_min_b,cq_max_b;
static bool cq_have_sample;
static void cq_open(void){++drill_debug_current.epoch;__DMB();}
static void cq_close(void){__DMB();++drill_debug_current.epoch;}
static void cq_fail(cq_fault f){
 cq_open();drill_debug_current.magic=0x43513833u;drill_debug_current.version=1;
 if(drill_debug_current.state==CQ_ZEROING&&cq_have_sample){drill_debug_current.peak_a_counts=cq_max_a-cq_min_a;drill_debug_current.peak_b_counts=cq_max_b-cq_min_b;}
 drill_debug_current.state=CQ_FAILED;drill_debug_current.fault=f;drill_debug_current.flags&=~2u;
 drill_dual_abort();drill_adc_abort();drill_spi_abort();drill_config.state=DC_FAILED;drill_config.error=DC_INTERLOCK;cq_close();
}
bool drill_current_active(void){return drill_debug_current.state==CQ_ZEROING||drill_debug_current.state==CQ_READY;}
static bool cq_interlock(void){
 const uint32_t a_low=(1u<<5)|(1u<<7)|(7u<<8),a_mask=(3u<<10)|(3u<<14)|(3u<<16)|(3u<<18)|(3u<<20)|(3u<<24),a_outputs=(1u<<10)|(1u<<14)|(1u<<16)|(1u<<18)|(1u<<20)|(1u<<24);
 return drill_dual_state==DU_STREAM&&dc_feedback_ready(&drill_config)&&
  TIM1->CCER==0u&&!(TIM1->BDTR&TIM_BDTR_MOE)&&
  (GPIOA->MODER&a_mask)==a_outputs&&(GPIOB->MODER&15u)==5u&&
  !(GPIOA->ODR&a_low)&&!(GPIOA->IDR&a_low)&&!(GPIOB->ODR&3u)&&!(GPIOB->IDR&3u)&&
  (GPIOA->ODR&(1u<<12))&&(GPIOA->IDR&(1u<<12))&&!(GPIOA->OTYPER&(1u<<12))&&
  (GPIOF->ODR&2u)&&(GPIOF->IDR&2u);
}
bool drill_current_bench_begin(const cq_profile *p,uint32_t now,bool qualified){
 if(!qualified||drill_debug_current.state!=CQ_IDLE)return false;
 if(!p||p->adc_reference_uv<2800000u||p->adc_reference_uv>3600000u||p->drv_reference_uv<3135000u||p->drv_reference_uv>3465000u||!p->limit_ma||p->limit_ma>2000u){cq_fail(CQ_ARGUMENT);return false;}
 /* 0.3 V/A is fixed by the checked gain readback. The 40mV zero band and
  * 50mV ADC ceiling margin are candidate commissioning margins, not accuracy. */
 uint32_t half=p->drv_reference_uv/2u,span=p->limit_ma*300u+40000u;
 if(half<span+250000u||half+span>p->drv_reference_uv-250000u||half+span>p->adc_reference_uv-50000u){cq_fail(CQ_RANGE);return false;}
 uint32_t actual;
 if(!cq_interlock()||!drill_spi_sampling_available()){cq_fail(CQ_INTERLOCK);return false;}
 if(!drill_timebase_get(&actual)||actual-now>30u){cq_fail(CQ_TIME);return false;}
 cq_lo=(uint32_t)(((uint64_t)250000u*4095u+p->adc_reference_uv-1u)/p->adc_reference_uv);
 cq_hi=(uint32_t)(((uint64_t)(p->drv_reference_uv-250000u)*4095u)/p->adc_reference_uv);if(cq_hi>4094u)cq_hi=4094u;
 cq_zero_lo=(uint32_t)(((uint64_t)(half-40000u)*4095u+p->adc_reference_uv-1u)/p->adc_reference_uv);
 cq_zero_hi=(uint32_t)(((uint64_t)(half+40000u)*4095u)/p->adc_reference_uv);
 cq_scale=(uint32_t)(((uint64_t)p->adc_reference_uv*1000u*65536u+614250000u)/1228500000u);
 cq_origin=cq_last_poll=cq_last_sample=actual;cq_have_sample=false;cq_sum_a=cq_sum_b=cq_sequence=0;
 cq_min_a=cq_min_b=4095;cq_max_a=cq_max_b=0;
 /* The caller may use a stack profile: copy fields before publication. */
 cq_profile saved=*p;
 drill_debug_current=(cq_record){.magic=0x43513833u,.version=1,.epoch=1,.state=CQ_ZEROING,.limit_ma=saved.limit_ma,.adc_reference_uv=saved.adc_reference_uv,.drv_reference_uv=saved.drv_reference_uv,.scale_ma_per_count_q16=cq_scale};cq_close();return true;
}
void drill_current_poll(uint32_t now){
 if(!drill_current_active())return;
 if(!cq_interlock()){cq_fail(CQ_INTERLOCK);return;}
 uint32_t fresh;if(!drill_timebase_get(&fresh)||fresh-now>30u){cq_fail(CQ_TIME);return;}now=fresh;
 if(now-cq_last_poll>0x7fffffffu){cq_fail(CQ_TIME);return;}cq_last_poll=now;
 if(now-cq_last_sample>100u||(drill_debug_current.state==CQ_ZEROING&&now-cq_origin>20000u)){cq_fail(CQ_TIME);return;}
}
static int32_t cq_nominal(uint16_t raw,uint32_t zero){
 int64_t value=((int64_t)raw*256-(int64_t)zero)*cq_scale;
 return value>=0?(int32_t)((value+8388608)/16777216):-(int32_t)((-value+8388608)/16777216);
}
void drill_current_feed(const du_sample *s,uint32_t now){
 if(!drill_current_active())return;
 drill_current_poll(now);if(!drill_current_active())return;now=cq_last_poll;
 if(!s){cq_fail(CQ_ARGUMENT);return;}
 /* armed_us is stream origin, not a per-pair ADC aperture timestamp. */
 if(now-s->observed_us>30u){cq_fail(CQ_TIME);return;}
 if(cq_have_sample&&(s->sequence!=cq_sequence+1u||s->observed_us-cq_last_sample==0u||s->observed_us-cq_last_sample>100u)){cq_fail(CQ_SEQUENCE);return;}
 if(s->ia_raw<cq_lo||s->ib_raw<cq_lo||s->ia_raw>cq_hi||s->ib_raw>cq_hi){cq_fail(CQ_RANGE);return;}
 if(drill_debug_current.state==CQ_ZEROING){
  if(s->ia_raw<cq_zero_lo||s->ia_raw>cq_zero_hi||s->ib_raw<cq_zero_lo||s->ib_raw>cq_zero_hi){cq_fail(CQ_OFFSET);return;}
  if(s->ia_raw<cq_min_a)cq_min_a=s->ia_raw;
  if(s->ia_raw>cq_max_a)cq_max_a=s->ia_raw;
  if(s->ib_raw<cq_min_b)cq_min_b=s->ib_raw;
  if(s->ib_raw>cq_max_b)cq_max_b=s->ib_raw;
  if((uint32_t)(cq_max_a-cq_min_a)>16u||(uint32_t)(cq_max_b-cq_min_b)>16u){cq_fail(CQ_NOISE);return;}
 }
 cq_have_sample=true;cq_sequence=s->sequence;cq_last_sample=s->observed_us;
 cq_open();drill_debug_current.last_sequence=s->sequence;drill_debug_current.last_observed_us=s->observed_us;
 if(drill_debug_current.state==CQ_ZEROING){
  cq_sum_a+=s->ia_raw;cq_sum_b+=s->ib_raw;++drill_debug_current.count;
  drill_debug_current.peak_a_counts=cq_max_a-cq_min_a;drill_debug_current.peak_b_counts=cq_max_b-cq_min_b;
  if(drill_debug_current.count==256u){drill_debug_current.zero_a_q8=cq_sum_a;drill_debug_current.zero_b_q8=cq_sum_b;drill_debug_current.flags=1u;drill_debug_current.state=CQ_READY;}
 }else{
  int32_t ia=cq_nominal(s->ia_raw,drill_debug_current.zero_a_q8),ib=cq_nominal(s->ib_raw,drill_debug_current.zero_b_q8),ic=-ia-ib;
  drill_debug_current.sense_a_ma=ia;drill_debug_current.sense_b_ma=ib;drill_debug_current.reconstructed_c_ma=ic;drill_debug_current.flags=7u;
  cq_close();int32_t limit=(int32_t)drill_debug_current.limit_ma;
  if(ia>limit||ia<-limit||ib>limit||ib<-limit||ic>limit||ic<-limit)cq_fail(CQ_LIMIT);
  return;
 }
 cq_close();
}
void drill_current_abort(void){if(drill_current_active())cq_fail(CQ_INTERLOCK);}
