#include "sstv.h"
#include "image.h"
#include "robot36.h"
#include "wav.h"
#include <math.h>
#include <stdint.h>

#define PI 3.14159265358979
#define AMPLITUDE 12000

static double phase = 0.0;

void sstv_tone(wav_t *wav, double freq, double duration_ms) {
    uint32_t total_samples = (uint32_t)((duration_ms / 1000.0) * wav->sample_rate);

    for (uint32_t i = 0; i < total_samples; i++) {
        double step = 2.0 * PI * freq / wav->sample_rate;
        phase += step;
        if (phase > 2.0 * PI)
            phase -= 2.0 * PI;

        int16_t sample = (int16_t)(AMPLITUDE * sin(phase));
        wave_write_sample(wav, sample);
    }
}

void sstv_vis_header(wav_t *wav) {
    uint8_t vis = 0x08;   
    uint8_t parity = 0;

    sstv_tone(wav, 1900, 300);
    sstv_tone(wav, 1200, 10);
    sstv_tone(wav, 1900, 300);

    sstv_tone(wav, 1200, 30);

    for (int i = 0; i < 7; i++) {
        uint8_t bit = (vis >> i) & 1;  
        if (bit) {
            sstv_tone(wav, 1100, 30);
            parity ^= 1;
        } else {
            sstv_tone(wav, 1300, 30);
        }
    }

    sstv_tone(wav, parity ? 1100 : 1300, 30);

    sstv_tone(wav, 1200, 30);
}

int sstv_encode_robot36(const char *input_image, const char *output_wav) {
    image_t img, resized;
    wav_t wav;

    if (!image_load(&img, input_image))
        return 0;

    if (!image_resize(&img, &resized, ROBOT36_WIDTH, ROBOT36_HEIGHT)) {
        image_free(&img);
        return 0;
    }

    if (!wave_open(&wav, output_wav, 44100)) {
        image_free(&img);
        image_free_raw(&resized);
        return 0;
    }

    sstv_vis_header(&wav);
    robot36_encode_image(&wav, resized.data);
    wave_close(&wav);

    image_free(&img);
    image_free_raw(&resized);

    return 1;
}