package modes

import (
	"fmt"
	"math"
)

// Downlink formats
const (
	DF0  = 0  // Short air-air surveillance
	DF4  = 4  // Surveillance altitude reply
	DF5  = 5  // Surveillance identity reply
	DF11 = 11 // All-call reply
	DF16 = 16 // Long air-air surveillance
	DF17 = 17 // Extended squitter
	DF18 = 18 // Extended squitter/non-transponder
	DF19 = 19 // Military extended squitter
	DF20 = 20 // Comm-B altitude reply
	DF21 = 21 // Comm-B identity reply
	DF24 = 24 // Comm-D (ELM)
)

// Type codes for DF17/18 messages
const (
	TC_IDENT_CATEGORY   = 1  // Aircraft identification and category
	TC_SURFACE_POS      = 5  // Surface position
	TC_AIRBORNE_POS_9   = 9  // Airborne position (w/ barometric altitude)
	TC_AIRBORNE_POS_20  = 20 // Airborne position (w/ GNSS height)
	TC_AIRBORNE_VEL     = 19 // Airborne velocity
	TC_AIRBORNE_POS_GPS = 22 // Airborne position (GNSS)
	TC_RESERVED         = 23 // Reserved
	TC_SURFACE_SYSTEM   = 24 // Surface system status
	TC_OPERATIONAL      = 31 // Aircraft operational status
)

// Aircraft represents decoded aircraft data
type Aircraft struct {
	ICAO24           uint32  // 24-bit ICAO address
	Callsign         string  // 8-character callsign
	Latitude         float64 // Decimal degrees
	Longitude        float64 // Decimal degrees
	Altitude         int     // Feet
	VerticalRate     int     // Feet/minute
	GroundSpeed      float64 // Knots
	Track            float64 // Degrees
	Heading          float64 // Degrees (magnetic)
	Category         string  // Aircraft category
	Emergency        string  // Emergency/priority status
	Squawk           string  // 4-digit squawk code
	OnGround         bool
	Alert            bool
	SPI              bool    // Special Position Identification
	NACp             uint8   // Navigation Accuracy Category - Position
	NACv             uint8   // Navigation Accuracy Category - Velocity
	SIL              uint8   // Surveillance Integrity Level
	BaroAltitude     int     // Barometric altitude
	GeomAltitude     int     // Geometric altitude
	SelectedAltitude int     // MCP/FCU selected altitude
	SelectedHeading  float64 // MCP/FCU selected heading
	BaroSetting      float64 // QNH in millibars
}

// Decoder handles Mode S message decoding
type Decoder struct {
	cprEvenLat  map[uint32]float64
	cprEvenLon  map[uint32]float64
	cprOddLat   map[uint32]float64
	cprOddLon   map[uint32]float64
	cprEvenTime map[uint32]int64
	cprOddTime  map[uint32]int64
}

// NewDecoder creates a new Mode S decoder
func NewDecoder() *Decoder {
	return &Decoder{
		cprEvenLat:  make(map[uint32]float64),
		cprEvenLon:  make(map[uint32]float64),
		cprOddLat:   make(map[uint32]float64),
		cprOddLon:   make(map[uint32]float64),
		cprEvenTime: make(map[uint32]int64),
		cprOddTime:  make(map[uint32]int64),
	}
}

// Decode processes a Mode S message
func (d *Decoder) Decode(data []byte) (*Aircraft, error) {
	if len(data) < 7 {
		return nil, fmt.Errorf("message too short: %d bytes", len(data))
	}

	df := (data[0] >> 3) & 0x1F
	icao := d.extractICAO(data, df)
	
	aircraft := &Aircraft{
		ICAO24: icao,
	}

	switch df {
	case DF4, DF20:
		aircraft.Altitude = d.decodeAltitude(data)
	case DF5, DF21:
		aircraft.Squawk = d.decodeSquawk(data)
	case DF17, DF18:
		return d.decodeExtendedSquitter(data, aircraft)
	}

	return aircraft, nil
}

// extractICAO extracts the ICAO address based on downlink format
func (d *Decoder) extractICAO(data []byte, df uint8) uint32 {
	// For most formats, ICAO is in bytes 1-3
	return uint32(data[1])<<16 | uint32(data[2])<<8 | uint32(data[3])
}

// decodeExtendedSquitter handles DF17/18 extended squitter messages
func (d *Decoder) decodeExtendedSquitter(data []byte, aircraft *Aircraft) (*Aircraft, error) {
	if len(data) < 14 {
		return nil, fmt.Errorf("extended squitter too short: %d bytes", len(data))
	}

	tc := (data[4] >> 3) & 0x1F

	switch {
	case tc >= 1 && tc <= 4:
		// Aircraft identification
		d.decodeIdentification(data, aircraft)
	case tc >= 5 && tc <= 8:
		// Surface position
		d.decodeSurfacePosition(data, aircraft)
	case tc >= 9 && tc <= 18:
		// Airborne position
		d.decodeAirbornePosition(data, aircraft)
	case tc == 19:
		// Airborne velocity
		d.decodeVelocity(data, aircraft)
	case tc >= 20 && tc <= 22:
		// Airborne position with GNSS
		d.decodeAirbornePosition(data, aircraft)
	case tc == 28:
		// Aircraft status
		d.decodeStatus(data, aircraft)
	case tc == 29:
		// Target state and status
		d.decodeTargetState(data, aircraft)
	case tc == 31:
		// Operational status
		d.decodeOperationalStatus(data, aircraft)
	}

	return aircraft, nil
}

// decodeIdentification extracts callsign and category
func (d *Decoder) decodeIdentification(data []byte, aircraft *Aircraft) {
	tc := (data[4] >> 3) & 0x1F
	
	// Category
	aircraft.Category = d.getAircraftCategory(tc, data[4]&0x07)
	
	// Callsign - 8 characters encoded in 6 bits each
	chars := "#ABCDEFGHIJKLMNOPQRSTUVWXYZ##### ###############0123456789######"
	callsign := ""
	
	// Extract 48 bits starting from bit 40
	for i := 0; i < 8; i++ {
		bitOffset := 40 + i*6
		byteOffset := bitOffset / 8
		bitShift := bitOffset % 8
		
		var charCode uint8
		if bitShift <= 2 {
			charCode = (data[byteOffset] >> (2 - bitShift)) & 0x3F
		} else {
			charCode = ((data[byteOffset] << (bitShift - 2)) & 0x3F) | 
			           (data[byteOffset+1] >> (10 - bitShift))
		}
		
		if charCode < 64 {
			callsign += string(chars[charCode])
		}
	}
	
	aircraft.Callsign = callsign
}

// decodeAirbornePosition extracts position from CPR encoded data
func (d *Decoder) decodeAirbornePosition(data []byte, aircraft *Aircraft) {
	tc := (data[4] >> 3) & 0x1F
	
	// Altitude
	altBits := (uint16(data[5])<<4 | uint16(data[6])>>4) & 0x0FFF
	aircraft.Altitude = d.decodeAltitudeBits(altBits, tc)
	
	// CPR latitude/longitude
	cprLat := uint32(data[6]&0x03)<<15 | uint32(data[7])<<7 | uint32(data[8])>>1
	cprLon := uint32(data[8]&0x01)<<16 | uint32(data[9])<<8 | uint32(data[10])
	oddFlag := (data[6] >> 2) & 0x01
	
	// Store CPR values for later decoding
	if oddFlag == 1 {
		d.cprOddLat[aircraft.ICAO24] = float64(cprLat) / 131072.0
		d.cprOddLon[aircraft.ICAO24] = float64(cprLon) / 131072.0
	} else {
		d.cprEvenLat[aircraft.ICAO24] = float64(cprLat) / 131072.0
		d.cprEvenLon[aircraft.ICAO24] = float64(cprLon) / 131072.0
	}
	
	// Try to decode position if we have both even and odd messages
	d.decodeCPRPosition(aircraft)
}

// decodeCPRPosition performs CPR global decoding
func (d *Decoder) decodeCPRPosition(aircraft *Aircraft) {
	evenLat, evenExists := d.cprEvenLat[aircraft.ICAO24]
	oddLat, oddExists := d.cprOddLat[aircraft.ICAO24]
	
	if !evenExists || !oddExists {
		return
	}
	
	evenLon := d.cprEvenLon[aircraft.ICAO24]
	oddLon := d.cprOddLon[aircraft.ICAO24]
	
	// CPR decoding algorithm
	dLat := 360.0 / 60.0
	j := math.Floor(evenLat*59 - oddLat*60 + 0.5)
	
	latEven := dLat * (math.Mod(j, 60) + evenLat)
	latOdd := dLat * (math.Mod(j, 59) + oddLat)
	
	if latEven >= 270 {
		latEven -= 360
	}
	if latOdd >= 270 {
		latOdd -= 360
	}
	
	// Choose the most recent position
	aircraft.Latitude = latOdd // Use odd for now, should check timestamps
	
	// Longitude calculation
	nl := d.nlFunction(aircraft.Latitude)
	ni := math.Max(nl-1, 1)
	dLon := 360.0 / ni
	m := math.Floor(evenLon*(nl-1) - oddLon*nl + 0.5)
	lon := dLon * (math.Mod(m, ni) + oddLon)
	
	if lon >= 180 {
		lon -= 360
	}
	
	aircraft.Longitude = lon
}

// nlFunction calculates the number of longitude zones
func (d *Decoder) nlFunction(lat float64) float64 {
	if math.Abs(lat) >= 87 {
		return 2
	}
	
	nz := 15.0
	a := 1 - math.Cos(math.Pi/(2*nz))
	b := math.Pow(math.Cos(math.Pi/180.0*math.Abs(lat)), 2)
	nl := 2 * math.Pi / math.Acos(1-a/b)
	
	return math.Floor(nl)
}

// decodeVelocity extracts speed and heading
func (d *Decoder) decodeVelocity(data []byte, aircraft *Aircraft) {
	subtype := (data[4]) & 0x07
	
	if subtype == 1 || subtype == 2 {
		// Ground speed
		ewRaw := uint16(data[5]&0x03)<<8 | uint16(data[6])
		nsRaw := uint16(data[7])<<3 | uint16(data[8])>>5
		
		ewVel := float64(ewRaw - 1)
		nsVel := float64(nsRaw - 1)
		
		if data[5]&0x04 != 0 {
			ewVel = -ewVel
		}
		if data[7]&0x80 != 0 {
			nsVel = -nsVel
		}
		
		aircraft.GroundSpeed = math.Sqrt(ewVel*ewVel + nsVel*nsVel)
		aircraft.Track = math.Atan2(ewVel, nsVel) * 180 / math.Pi
		if aircraft.Track < 0 {
			aircraft.Track += 360
		}
	}
	
	// Vertical rate
	vrSign := (data[8] >> 3) & 0x01
	vrBits := uint16(data[8]&0x07)<<6 | uint16(data[9])>>2
	if vrBits != 0 {
		aircraft.VerticalRate = int(vrBits-1) * 64
		if vrSign != 0 {
			aircraft.VerticalRate = -aircraft.VerticalRate
		}
	}
}

// decodeAltitude extracts altitude from Mode S altitude reply
func (d *Decoder) decodeAltitude(data []byte) int {
	altCode := uint16(data[2])<<8 | uint16(data[3])
	return d.decodeAltitudeBits(altCode>>3, 0)
}

// decodeAltitudeBits converts altitude code to feet
func (d *Decoder) decodeAltitudeBits(altCode uint16, tc uint8) int {
	if altCode == 0 {
		return 0
	}
	
	// Gray code to binary conversion
	var n uint16
	for i := uint(0); i < 12; i++ {
		n ^= altCode >> i
	}
	
	alt := int(n)*25 - 1000
	
	if tc >= 20 && tc <= 22 {
		// GNSS altitude
		return alt
	}
	
	return alt
}

// decodeSquawk extracts squawk code
func (d *Decoder) decodeSquawk(data []byte) string {
	code := uint16(data[2])<<8 | uint16(data[3])
	return fmt.Sprintf("%04o", code>>3)
}

// getAircraftCategory returns human-readable aircraft category
func (d *Decoder) getAircraftCategory(tc uint8, ca uint8) string {
	switch tc {
	case 1:
		return "Reserved"
	case 2:
		switch ca {
		case 1:
			return "Surface Emergency Vehicle"
		case 3:
			return "Surface Service Vehicle"
		case 4, 5, 6, 7:
			return "Ground Obstruction"
		default:
			return "Surface Vehicle"
		}
	case 3:
		switch ca {
		case 1:
			return "Glider/Sailplane"
		case 2:
			return "Lighter-than-Air"
		case 3:
			return "Parachutist/Skydiver"
		case 4:
			return "Ultralight/Hang-glider"
		case 6:
			return "UAV"
		case 7:
			return "Space Vehicle"
		default:
			return "Light Aircraft"
		}
	case 4:
		switch ca {
		case 1:
			return "Light < 7000kg"
		case 2:
			return "Medium 7000-34000kg"
		case 3:
			return "Medium 34000-136000kg"
		case 4:
			return "High Vortex Large"
		case 5:
			return "Heavy > 136000kg"
		case 6:
			return "High Performance"
		case 7:
			return "Rotorcraft"
		default:
			return "Aircraft"
		}
	default:
		return "Unknown"
	}
}

// decodeStatus handles aircraft status messages
func (d *Decoder) decodeStatus(data []byte, aircraft *Aircraft) {
	subtype := data[4] & 0x07
	
	if subtype == 1 {
		// Emergency/priority status
		emergency := (data[5] >> 5) & 0x07
		switch emergency {
		case 0:
			aircraft.Emergency = "None"
		case 1:
			aircraft.Emergency = "General Emergency"
		case 2:
			aircraft.Emergency = "Lifeguard/Medical"
		case 3:
			aircraft.Emergency = "Minimum Fuel"
		case 4:
			aircraft.Emergency = "No Communications"
		case 5:
			aircraft.Emergency = "Unlawful Interference"
		case 6:
			aircraft.Emergency = "Downed Aircraft"
		}
	}
}

// decodeTargetState handles target state and status messages
func (d *Decoder) decodeTargetState(data []byte, aircraft *Aircraft) {
	// Selected altitude
	altBits := uint16(data[5]&0x7F)<<4 | uint16(data[6])>>4
	if altBits != 0 {
		aircraft.SelectedAltitude = int(altBits)*32 - 32
	}
	
	// Barometric pressure setting
	baroBits := uint16(data[7])<<1 | uint16(data[8])>>7
	if baroBits != 0 {
		aircraft.BaroSetting = float64(baroBits)*0.8 + 800
	}
}

// decodeOperationalStatus handles operational status messages
func (d *Decoder) decodeOperationalStatus(data []byte, aircraft *Aircraft) {
	// Navigation accuracy categories
	aircraft.NACp = (data[7] >> 4) & 0x0F
	aircraft.NACv = data[7] & 0x0F
	aircraft.SIL = (data[8] >> 6) & 0x03
}

// decodeSurfacePosition handles surface position messages
func (d *Decoder) decodeSurfacePosition(data []byte, aircraft *Aircraft) {
	aircraft.OnGround = true
	
	// Movement
	movement := uint8(data[4]&0x07)<<4 | uint8(data[5])>>4
	if movement > 0 && movement < 125 {
		aircraft.GroundSpeed = d.decodeGroundSpeed(movement)
	}
	
	// Track
	trackValid := (data[5] >> 3) & 0x01
	if trackValid != 0 {
		trackBits := uint16(data[5]&0x07)<<4 | uint16(data[6])>>4
		aircraft.Track = float64(trackBits) * 360.0 / 128.0
	}
	
	// CPR position (similar to airborne)
	cprLat := uint32(data[6]&0x03)<<15 | uint32(data[7])<<7 | uint32(data[8])>>1
	cprLon := uint32(data[8]&0x01)<<16 | uint32(data[9])<<8 | uint32(data[10])
	oddFlag := (data[6] >> 2) & 0x01
	
	if oddFlag == 1 {
		d.cprOddLat[aircraft.ICAO24] = float64(cprLat) / 131072.0
		d.cprOddLon[aircraft.ICAO24] = float64(cprLon) / 131072.0
	} else {
		d.cprEvenLat[aircraft.ICAO24] = float64(cprLat) / 131072.0
		d.cprEvenLon[aircraft.ICAO24] = float64(cprLon) / 131072.0
	}
	
	d.decodeCPRPosition(aircraft)
}

// decodeGroundSpeed converts movement field to ground speed
func (d *Decoder) decodeGroundSpeed(movement uint8) float64 {
	if movement == 1 {
		return 0
	} else if movement >= 2 && movement <= 8 {
		return float64(movement-2)*0.125 + 0.125
	} else if movement >= 9 && movement <= 12 {
		return float64(movement-9)*0.25 + 1.0
	} else if movement >= 13 && movement <= 38 {
		return float64(movement-13)*0.5 + 2.0
	} else if movement >= 39 && movement <= 93 {
		return float64(movement-39)*1.0 + 15.0
	} else if movement >= 94 && movement <= 108 {
		return float64(movement-94)*2.0 + 70.0
	} else if movement >= 109 && movement <= 123 {
		return float64(movement-109)*5.0 + 100.0
	} else if movement == 124 {
		return 175.0
	}
	return 0
}