/*
 * WESPION.c
 *
 *  Revised on: Dec 16, 2023
 *      Author: HwangSungWoo
 */



#include "WESPION.h"

// ── FW 빌드 정보 ──
FW_BuildInfo_t FW_Build = {
  .version = "2.0.0-dev.1",
  .commit  = "85dd0e3",
  .date    = __DATE__,
  .time    = __TIME__,
};

WP_SwVersionTypeDef WespionVer;
WP_EncInitTypeDef WP_EncInit={0,};
WP_ForcedCaliTypeDef WP_ForcedCali={0,};
WP_MachineTypeDef WP_Machine={0,};
WP_GymTypeDef WP_Gym={0,};
WP_WeightTypeDef  WP_Weight={0,};
WP_UserSettingTypeDef WP_UserSetting={0,};
WP_RegenRTypeDef WP_RegenR={0,};
WP_DeratingTypeDef WP_Derating={0,};
WP_LEDCtrlTypeDef WP_LEDCtrl = {0,};
//WP_FfScaleTypeDef FfScale={0,};

void Initialize_WESPION(void)
{
  // 25.05.05 - 다시 소스코드에서 관리하는 방식으로 변경 -> 이렇게 해야 DFU시에 같이 변경됨!
  WespionVer.VerMajer = 0x02;
  WespionVer.VerMiner = 0x00;
  WespionVer.VerSub   = 0x00;
  WespionVer.VerPre   = 0x02;  // dev.2

  WespionVer.VerYear = 2026;
  WespionVer.VerMonth = 07;
  WespionVer.VerDate = 05;

//  WespionVer.VerMajer = RAM_Data.VerMajer;  ///VER_MAJER
//  WespionVer.VerMiner = RAM_Data.VerMiner;  ///VER_MINER
//  WespionVer.VerSub = RAM_Data.VerSub;      ///VER_SUB
//
//  WespionVer.VerYear = RAM_Data.VerYear;    ///VER_YEAR
//  WespionVer.VerMonth = RAM_Data.VerMonth;  ///VER_MONTH
//  WespionVer.VerDate = RAM_Data.VerDate;    ///VER_DATE

  MotorCtrl_Initialize();

  Initialize_App();
  Initialize_Machine();
  Initialize_Gym();
  Initialize_Weight();

  Initialize_UserSetting();

  Initialize_RegenR();
  Initialize_Derating();
//  LED_Queue_Init();

  // Temporary initializations for Test
  Status_VoltRprt = OFF;
}

void Initialize_App(void)
{
  pinMode(56, OUTPUT);  // LED1
  pinMode(57, OUTPUT);  // LED2
  pinMode(87, OUTPUT);  // LED3
  pinMode(88, OUTPUT);  // LED4
  pinMode(89, OUTPUT);  // LED5
  pinMode(90, OUTPUT);  // LED6
#if   HW_VER < 3
  pinMode(88, OUTPUT);  // LED4
  pinMode(89, OUTPUT);  // LED5
  pinMode(90, OUTPUT);  // LED6
  pinMode(91, OUTPUT);  // LED7
#endif
  digitalWrite(56, 1);
  digitalWrite(57, 1);
  digitalWrite(87, 1);
#if   HW_VER < 3
  digitalWrite(88, 1);
  digitalWrite(89, 1);
  digitalWrite(90, 1);
  digitalWrite(91, 1);
#endif
}

void Initialize_Machine(void)
{
//  WP_Machine.SensorGearRatio = 3.0f;
//  WP_Machine.SensorGearRatioInv = 1.0f / WP_Machine.SensorGearRatio;
//  WP_Machine.SpoolDiameter = 60.0f;        // [mm]
//  WP_Machine.MaxWeight = 10.0f;            // [kg]인데 현재는 A, 테스트 안전을 위해 막아둠
//  WP_Machine.CableMaxLength = 2650;     // [mm]
//  WP_Machine.SoftWindingHeight = 20;    // [mm]
//
//  WP_Machine.Scale_Weight2Current = 0.5f;  // [kg/A] -> C샘플, BS-105-10 기준 약 2
//  WP_Machine.Scale_Current2Weight = 1.0f / WP_Machine.Scale_Weight2Current;
//  WP_Machine.DefaultWeight = 0.6;       // [A] -> 나중엔 [kg]로 바꿔야 함
//  WP_Machine.FrictionWeight = 0.45;     // [A] -> 0.7kg에 해당
//  WP_Machine.FrictionSpeedPlus = -20.0f;
//  WP_Machine.FrictionSpeedMinus = 90.0f;

  WP_Machine.SensorGearRatio = RAM_Data.Machine_SensorGearRatio;                          ///MACHINE_SENSOR_GEAR_RATIO
  WP_Machine.SensorGearRatioInv = 1.0f / WP_Machine.SensorGearRatio;
  WP_Machine.SpoolDiameter = RAM_Data.Machine_SpoolDiameter;                    // [mm]   ///MACHINE_SPOOL_DIAMETER
  WP_Machine.MaxWeight = RAM_Data.Machine_MaxWeight;                            // [kg]   ///MACHINE_MAX_WEIGHT
  WP_Machine.CableMaxLength = (uint16_t)RAM_Data.Machine_CableMaxLength;        // [mm]   ///MACHINE_CABLE_MAX_LENGTH
  WP_Machine.SoftWindingHeight = RAM_Data.Machine_SoftWindingHeight;            // [mm]   ///MACHINE_SOFT_WINDING_HEIGHT

  WP_Machine.Scale_Weight2Current = RAM_Data.Machine_Scale_Weight2Current;      // [kg/A] ///MACHINE_SCALE_WEIGHT2CURRENT
  WP_Machine.Scale_Current2Weight = 1.0f / WP_Machine.Scale_Weight2Current;
  WP_Machine.DefaultWeight = RAM_Data.Machine_DefaultWeight;                    // [A] -> 나중엔 [kg]로 바꿔야 함 ///MACHINE_DEFAULT_WEIGHT
  WP_Machine.FrictionWeight = RAM_Data.Machine_FrictionWeight;                  // [A] -> 0.7kg에 해당         ///MACHINE_FRICTION_WEIGHT
  WP_Machine.FrictionSpeedPlus = RAM_Data.Machine_FrictionSpeedPlus;                      ///MACHINE_FRICTION_SPEED_PLUS
  WP_Machine.FrictionSpeedMinus = RAM_Data.Machine_FrictionSpeedMinus;                    ///MACHINE_FRICTION_SPEED_MINUS

  WP_Machine.MaxCurrent = 30.0f;
}

void Initialize_Gym(void)
{
  WP_Gym.AutoWeightRatio[L]= 1.0f;
  WP_Gym.AutoWeightRatio[R]= 1.0f;

  GymRange_Init();
  WP_Gym.F_stop = 0.0f;      //  3/19 kg 단위로 업데이트 하긴 했음
  WP_Gym.F_min = 1.1f;    // 25.03.14 - 금속 기어마찰 의심돼서 1.1로 올려봄
  WP_Gym.F_shock = 1.50f;  // 25.03.14 - 금속 기어마찰 의심돼서 올려봄
  WP_Gym.F_idle = 2.0f;    //  25.03.24 - 가동범위 인식 떄 너무 가벼워서 올림

  WP_Gym.F_EccSet = 0.0f;      //  초기값임.
  WP_Gym.F_EccLimit = 0.5f * (WP_Machine.MaxCurrent * WP_Machine.Scale_Current2Weight);
  WP_Gym.EccSpdUp = 2;
  WP_Gym.EccSpdDown = -2;

  WP_Gym.AutoWeightRatio[L] = 1.0f;
  WP_Gym.AutoWeightRatio[R] = 1.0f;

}
//  (25.05.05) 가동범위 초기화 커맨드를 위해 따로 분리함.
void GymRange_Init(void)
{
  WP_Gym.Region.H_gnd = -2700;
  // [mm] (25.05.01) 케이블 최대 길이 만큼은 무조건 감기도록 함.
  // (25.05.02) 회전관성과 함께 오버플로우인지 뭔지,계속 회전하는 현상
  WP_Gym.Region.H_base = 0;
  WP_Gym.Region.H_shock = 30;
  WP_Gym.Region.L_soft = 30;     //  가동범위 설정 시 불편함을 줄이기 위해  값을 줄여야 우리함.
  WP_Gym.Region.L_soft_inv = 1.0f / WP_Gym.Region.L_soft;
  WP_Gym.Region.H_LoSoft[L] = WP_Gym.Region.H_shock;   // 초기값임 -> 가동범위 세팅시 갱신 필요!
  WP_Gym.Region.H_LoSoft[R] = WP_Gym.Region.H_shock;
  WP_Gym.Region.L_RangeMin = 50.0f;
  WP_Gym.Region.RangeLo[L] = WP_Gym.Region.H_LoSoft[L] + WP_Gym.Region.L_soft;                 // 초기값임 -> 가동범위 세팅시 갱신 필요!
  WP_Gym.Region.RangeLo[R] = WP_Gym.Region.H_LoSoft[L] + WP_Gym.Region.L_soft;            //  가동범위 위에서 무게가 가벼워지면, 방심하다가 조금 내려갈 때 무게가 증가하면서 위험해질 수 있으므로 일단 구현 xx
  WP_Gym.Region.RangeHi[L] = 1800.0f;             // 임의의 값임 -> 향후 수정 필요!!
  WP_Gym.Region.RangeHi[R] = 1800.0f;
  WP_Gym.Region.H_HiSoft[L] = WP_Gym.Region.RangeHi[L] + WP_Gym.Region.L_soft;
  WP_Gym.Region.H_HiSoft[R] = WP_Gym.Region.RangeHi[R] + WP_Gym.Region.L_soft;
}
void Initialize_Weight(void)
{
//  Common Parameters
  WP_Weight.RefSpdMove = 10;     // mm/s
  WP_Weight.RefSpdStop = 2.5;

  WP_Gym.WeightSet[L] = 0.0f;
  WP_Gym.WeightSet[R] = 0.0f;
  WP_Gym.WeightMode[L] = 0;
  WP_Gym.WeightMode[R] = 0;
  WP_Weight.Ecc_Step = 0.0010;     //  WeightController 주기(16÷2kHz) 당 변화량 -> 주기가 부정확 함.

  //  Hydraulic Mode 초기값
  WP_Weight.Hydro.Vref = 300.0f;   // 기준속도 (mm/s)
  WP_Weight.Hydro.Vmax = 600.0f;   // 최대속도 cap (mm/s)
  WP_Weight.Hydro.n    = 1.0f;     // 속도 지수 (1.0=선형)
  WP_Weight.Hydro.minRatio = 0.1f; // 최소 텐션 (정지 시 설정무게의 10%)

  // Isokinetic
  WP_Weight.Iso.Vtarget   = 200.0f;  // 목표속도 (mm/s)
  WP_Weight.Iso.Kp        = 0.10f;   // 비례 게인 [kg/(mm/s)]
  WP_Weight.Iso.Ki        = 1.0f;    // 적분 게인 — P가 즉각 잡고, I는 잔여 오차 처리
  WP_Weight.Iso.DecayRate = 20.0f;   // 하강 감쇠 속도 (kg/s), 20=50ms에 1kg
  WP_Weight.Iso.Fmin      = 0.0f;    // 최소 부하 (kg), 0=F_idle까지 풀림
  WP_Weight.Iso.Fmax      = 0.0f;    // 최대 부하 (kg), 0=TargetWeight 사용

  // Vibration
  WP_Weight.Vib.Freq        = 20.0f;   // 20Hz
  WP_Weight.Vib.AmplitudeKg = 3.0f;   // 진폭 3kg
  WP_Weight.Vib.MaxRatio    = 0.30f;  // 기본무게의 30% 상한

  // 공통 안전장치
  WP_Weight.Safety.SlewRate_kgPerSec = 50.0f;  // 50kg/s → 0→10kg에 200ms
  WP_Weight.Safety.IqLimit = 0.0f;             // 비활성 (MaxCurrent가 최종 보호)

//  WeightChanger
//  WP_Weight.ChangePeriod = 10.0f;    //  msec
//  WP_Weight.DiffSmall = 0.1f;       //  kg인데 현재는 A
//  WP_Weight.DiffLarge = 1.0f;
//  WP_Weight.ChangeMin = 1.0f;      //  kg/s인데 현재는 A/s
//  WP_Weight.ChangeMax = 2.0f;

//  WeightOnOff
  WP_Weight.Ctrl.MotionAutoActive = 0;         // 초기엔 MotionAutoOn 불가
  WP_Weight.Ctrl.MotionAutoStatus = 0;
  WP_Weight.Ctrl.OnOffStatus[L] = 0.0f;   // 초기값임.
  WP_Weight.Ctrl.OnOffStatus[R] = 0.0f;
  WP_Weight.Ctrl.OnOffScale[L] = 0.0f;   // 초기값임.
  WP_Weight.Ctrl.OnOffScale[R] = 0.0f;
  WP_Weight.Ctrl.OnOffStep = 0.00005;    // 적당한지 테스트 필요, 향후 스케쥴러로 변경!


//  WeightFeel
  WP_Weight.FfScale.Temp[L] = 0;
  WP_Weight.FfScale.Temp[R] = 0;
  WP_Weight.FfScale.Min = 0.1f;     // 0.3에서 일단 낮춤
  WP_Weight.FfScale.Max = 1.4f;
  WP_Weight.FfScale.Step = 0.00016;   // 무게 단위 후처리 변동으로 2배처리함 (3/19)
}

void Initialize_UserSetting(void)
{
  WP_UserSetting.TimeCali = 200;       // [msec]
  WP_UserSetting.TimeSafetyOFF = 3000;
  WP_LEDCtrl.R = 8;
  WP_LEDCtrl.G = 4;
  WP_LEDCtrl.B = 80;
}

void Initialize_RegenR(void)
{
  WP_RegenR.Vdc_Limit = 56.0f;    // 4/8 과충전(추정) 발생 이후로 타이트하게 수정 (기존 58)
  WP_RegenR.Vdc_Off = 55.0f;
  WP_RegenR.Vdc_margin_Inv = 1.0f / (WP_RegenR.Vdc_Limit - WP_RegenR.Vdc_Off);

  WP_RegenR.Idc_Charge_Limit = -10.0f;
  WP_RegenR.Idc_Off = -6.0f;
  WP_RegenR.Idc_margin_Inv = 1.0f / (WP_RegenR.Idc_Charge_Limit - WP_RegenR.Idc_Off);

  WP_RegenR.alpha_Vdc = 0.1f;    // 조정 필요!! (현재 임의 값 3/3)
//  WP_RegenR.alpha_Idc = 0.00001f;     // 25.02.25 최소한의 안정된 값을 갖기 위한 실험적 값
  WP_RegenR.alpha_Idc = 0.005f;     // 25.03.14 응답이 너무 느려서 키웠음
  WP_RegenR.RegenScale_Vdc = 1.0f;
  WP_RegenR.RegenScale_Idc = 1.0f;
  WP_RegenR.PWM_Vdc = 0;
  WP_RegenR.PWM_Idc = 0;
  WP_RegenR.PWM = 0;

  WP_RegenR.DebugStatus = 0;    // 1: debug ON
  WP_RegenR.Status = 0;

}

void Initialize_Derating(void)
{
  WP_Derating.DebugStatus = 1;
  WP_Derating.Status = 0;

  WP_Derating.Iq_x10[L] = 0;    // 0.1A 단위 - 부동소수점 연산 제거용
  WP_Derating.Iq_x10[R] = 0;
  WP_Derating.Iq_x10_Debug[L] = 0;
  WP_Derating.Iq_x10_Debug[R] = 0;
  WP_Derating.Iq_x10_cont[L] = 75;    // 7.5A 임의의 경험적 초기값 (3/22)
  WP_Derating.Iq_x10_cont[R] = WP_Derating.Iq_x10_cont[L];
  WP_Derating.P_diss[L] = WP_Derating.Iq_x10_cont[L] * WP_Derating.Iq_x10_cont[L];
  WP_Derating.P_diss[L] = WP_Derating.P_diss[R];
  WP_Derating.Iq_x10_Lim_Min = 50;    // Derating 최대 적용 전류 : 냉각에 영향

  WP_Derating.deltaT[L] = 0;
  WP_Derating.deltaT[R] = 0;
  WP_Derating.deltaT_max = 50000000;    // 일단 임의값 -> Iq^2과 시간 고려해야 함

  WP_Derating.DeratingScale[L] = 0;
  WP_Derating.DeratingScale[R] = 0;

//  초기값은 머신의 최대치를 따름 -> 초기화 순서 지켜져야 함!!
  WP_Derating.Iq_Lim[L] = WP_Machine.MaxCurrent;
  WP_Derating.Iq_Lim[R] = WP_Machine.MaxCurrent;
}