/*
 * ТЕСТ РАСЧЕТА УГЛА ПАРУСА
 * Формула: P = F - (k × Fn^0.999988 + b)
 * F - угол флюгера (0-360)
 * Fn = F (нормализация не нужна, берем F как есть)
 * P - угол паруса (0-180)
 */

#include <Arduino.h>
#include <ESP32Servo.h>
#include <math.h>

Servo servo;
const int servoPin = 26;

// Структура для сектора
struct Sector {
  int minAngle;        // Минимальный угол сектора
  int maxAngle;        // Максимальный угол сектора  
  float k;             // Коэффициент k
  float b;             // Коэффициент b
};

// Таблица секторов D (0-360)
const Sector sectors[] = {
  {20, 105, 0.0588272, 11.8235},
  {105, 120, 1.80013, -171.00246},
  {120, 135, 0.333357, 4.99948},
  {135, 165, 0.666715, -40.00116},
  {165, 180, 1.33343, -150.00280},
  {180, 195, 1.33343, -150.00305},
  {195, 225, 0.666718, -20.00165},
  {225, 240, 0.33336, 54.99905},
  {240, 255, 1.80014, -297.00543},
  {255, 340, 0.0588283, 146.99979}
};

const int numSectors = sizeof(sectors) / sizeof(sectors[0]);
const float POWER = 0.999988f;

// Найти сектор для угла F
const Sector* findSector(int F) {
  for (int i = 0; i < numSectors; i++) {
    if (F >= sectors[i].minAngle && F < sectors[i].maxAngle) {
      return &sectors[i];
    }
  }
  return NULL;
}

// Рассчитать угол паруса P
float calculateSailAngle(int F) {
  // Запретные зоны 0-20 и 340-360
  if (F < 20 || F >= 340) {
    return 90.0f;
  }
  
  // Найти сектор
  const Sector* sector = findSector(F);
  if (sector == NULL) {
    return 90.0f;
  }
  
  // Fn = F (берем как есть)
  float Fn = (float)F;
  
  // Fn^0.999988
  float FnPowered = powf(Fn, POWER);
  
  // k × Fn^0.999988 + b
  float correction = (sector->k * FnPowered) + sector->b;
  
  // P = F - correction
  float P = Fn - correction;
  
  // Нормализовать к 0-360
  while (P < 0) P += 360.0f;
  while (P >= 360.0f) P -= 360.0f;
  
  // Преобразовать к 0-180 для паруса
  if (P > 180.0f) {
    P = 360.0f - P;
  }
  
  // Ограничить 0-180
  if (P < 0) P = 0;
  if (P > 180) P = 180;
  
  return P;
}

// Тестировать один угол
void testAngle(int F) {
  float P = calculateSailAngle(F);
  
  Serial.print("Ветер F = ");
  Serial.print(F);
  Serial.print("°");
  
  // Показать сектор
  const Sector* sector = findSector(F);
  if (sector) {
    Serial.print("  Сектор: ");
    Serial.print(sector->minAngle);
    Serial.print("-");
    Serial.print(sector->maxAngle);
    Serial.print("°  k=");
    Serial.print(sector->k, 6);
    Serial.print("  b=");
    Serial.print(sector->b, 6);
  } else if (F < 20 || F >= 340) {
    Serial.print("  (запретная зона)");
  } else {
    Serial.print("  (вне секторов)");
  }
  
  Serial.print("  Парус P = ");
  Serial.print(P, 4);
  Serial.println("°");
  
  servo.write((int)round(P));
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  
  servo.attach(servoPin, 500, 2400);
  
  Serial.println("\n==================================");
  Serial.println("ТЕСТ РАСЧЕТА УГЛА ПАРУСА");
  Serial.println("Формула: P = F - (k × Fn^0.999988 + b)");
  Serial.println("где Fn = F");
  Serial.println("F - угол флюгера (0-360)");
  Serial.println("P - угол паруса (0-180)");
  Serial.println("Запретные зоны: 0-20°, 340-360° → P=90°");
  Serial.println("==================================");
  
  Serial.println("\nСекторы и коэффициенты:");
  Serial.println("Диапазон F   |    k         |    b");
  Serial.println("-------------|--------------|--------------");
  
  for (int i = 0; i < numSectors; i++) {
    Serial.print(sectors[i].minAngle);
    Serial.print("-");
    Serial.print(sectors[i].maxAngle);
    Serial.print("       | ");
    Serial.print(sectors[i].k, 7);
    Serial.print("  | ");
    Serial.print(sectors[i].b, 7);
    Serial.println();
  }
  
  Serial.print("\nСтепень power = ");
  Serial.println(POWER, 6);
  
  Serial.println("\nВведите угол флюгера F (0-360):");
}

void loop() {
  if (Serial.available()) {
    String input = Serial.readStringUntil('\n');
    input.trim();
    
    if (input.length() > 0) {
      int F = input.toInt();
      if (F >= 0 && F <= 360) {
        testAngle(F);
      } else {
        Serial.println("Ошибка: F должен быть 0-360");
      }
      
      Serial.println("\nВведите следующий угол F (0-360):");
    }
  }
}