#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;
static double sample_accum = 0.0;  // accumulator

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

    double step = 2.0 * PI * freq / wav->sample_rate;
    for (uint32_t i = 0; i < total_samples; i++) {
        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_ex(wav_t *wav, uint8_t vis_code) {
  // reset accumulator
    phase        = 0.0;
    sample_accum = 0.0;
    
    /* preamble */
    sstv_tone(wav, 1900, 300);
    sstv_tone(wav, 1200,  10);
    sstv_tone(wav, 1900, 300);

    sstv_tone(wav, 1200, 30);

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

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

    /* stop bit */
    sstv_tone(wav, 1200, 30);
}

void sstv_vis_header(wav_t *wav) {
    sstv_vis_header_ex(wav, 0x08);
}

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;
}